public static void Cross(ref Vector3F left, ref Vector3F right, ref Vector3F result)
{
result.X = left.Y * right.Z - left.Z * right.Y;
result.Y = left.Z * right.X - left.X * right.Z;
result.Z = left.X * right.Y - left.Y * right.X;
}
public static void Dot(ref Vector3F left, ref Vector3F right, out float result)
{
result = left.X * right.X + left.Y * right.Y + left.Z * right.Z;
}
public static Vector3F Cross(Vector3F left, Vector3F right)
{
return(new Vector3F(left.Y * right.Z - left.Z * right.Y,
left.Z * right.X - left.X * right.Z,
left.X * right.Y - left.Y * right.X));
}
public Vector3F(Vector3F v)
{
Pos[0] = v.X;
Pos[1] = v.Y;
Pos[2] = v.Z;
}
public static float Dot(Vector3F left, Vector3F right)
{
return(left.X * right.X + left.Y * right.Y + left.Z * right.Z);
}
public static float Distance(Vector3F v1, Vector3F v2)
{
return((float)System.Math.Sqrt((v2.X - v1.X) * (v2.X - v1.X) + (v2.Y - v1.Y) * (v2.Y - v1.Y) + (v2.Z - v1.Z) * (v2.Z - v1.Z)));
}
public static float DistanceSquared(Vector3F v1, Vector3F v2)
{
return((v2.X - v1.X) * (v2.X - v1.X) + (v2.Y - v1.Y) * (v2.Y - v1.Y) + (v2.Z - v1.Z) * (v2.Z - v1.Z));
}
/// <summary>
/// Build a scaling matrix
/// </summary>
/// <param name="scale">Scale factors for x,y and z axes</param>
/// <returns>A scaling matrix</returns>
public static Matrix4F Scale(Vector3F scale)
{
return(Scale(scale.X, scale.Y, scale.Z));
}
public static Vector3F LinInterp3DEX(Vector3F p1, Vector3F p2, float p2ZShift, float t)
{
return(new Vector3F(LinInterp(p1.X, p2.X, t), LinInterp(p1.Y, p2.Y, t), LinInterp(p1.Z, p2.Z + p2ZShift, t)));
}
/// <summary>
/// Creates a translation matrix.
/// </summary>
/// <param name="vector">The translation vector.</param>
/// <param name="result">The resulting Matrix4F instance.</param>
public static void CreateTranslation(ref Vector3F vector, out Matrix4F result)
{
result = Identity;
result.Row3 = new Vector4F(vector.X, vector.Y, vector.Z, 1);
}
/// <summary>
/// Construct a new Quaternion from vector and w components
/// </summary>
/// <param name="v">The vector part</param>
/// <param name="w">The w part</param>
public Quaternion(Vector3F v, float w)
{
this.xyz = v;
this.w = w;
}
/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static Quaternion Slerp(Quaternion q1, Quaternion q2, float blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == 0.0f)
{
return(q1);
}
float cosHalfAngle = q1.W * q2.W + Vector3F.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0f)
{
q2.Xyz = q2.Xyz * -1;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
float halfAngle = (float)System.Math.Acos(cosHalfAngle);
float sinHalfAngle = (float)System.Math.Sin(halfAngle);
float oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
Quaternion result = new Quaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public static void Mult(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3F.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3F.Dot(left.Xyz, right.Xyz));
}
public static Quaternion Mult(Quaternion left, Quaternion right)
{
return(new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3F.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3F.Dot(left.Xyz, right.Xyz)));
}
