public void Normalize()
{
REAL invLength = DMath.SafeInvSqrt(1.0, x * x + y * y + z * z);
x *= invLength;
y *= invLength;
z *= invLength;
}
/// <summary>
/// Slerp between two vectors arc.
/// </summary>
public static Vector3d Slerp(Vector3d from, Vector3d to, REAL t)
{
if (t < 0.0f)
{
t = 0.0f;
}
if (t > 1.0f)
{
t = 1.0f;
}
if (t == 0.0f)
{
return(from);
}
if (t == 1.0f)
{
return(to);
}
if (to.x == from.x && to.y == from.y && to.z == from.z)
{
return(to);
}
REAL m = from.Magnitude * to.Magnitude;
if (DMath.IsZero(m))
{
return(Vector3d.Zero);
}
REAL theta = Math.Acos(Dot(from, to) / m);
if (theta == 0.0)
{
return(to);
}
REAL sinTheta = Math.Sin(theta);
REAL st1 = Math.Sin((1.0 - t) * theta) / sinTheta;
REAL st = Math.Sin(t * theta) / sinTheta;
Vector3d v = new Vector3d();
v.x = from.x * st1 + to.x * st;
v.y = from.y * st1 + to.y * st;
v.z = from.z * st1 + to.z * st;
return(v);
}
public static REAL Distance(Vector3d v0, Vector3d v1)
{
return(DMath.SafeSqrt(SqrDistance(v0, v1)));
}
