Useful geometry functions -)

void ProjectPointOnPlane(const Vector3 & p, const Plane & plane, Vector3 & projected)
{
    float d = plane.DistanceToPoint(p);
    projected = p - d*plane.n;
}


float CalcVectorAngle(const Vector2 & a, const Vector2 & b)
{
    float c = atan2f(b.y,b.x) - atan2f(a.y,a.x);

    if (c < -PI)
        c += 2.f*PI;
    else
        if (c > PI)
            c -= 2.f*PI;

    return c;
}