/// <summary>
/// Construct a quaternion that rotates from one direction to another
/// </summary>
/// <param name="startingDirection"></param>
/// <param name="endingDirection"></param>
public Quaternion(Vector3 startingDirection, Vector3 endingDirection)
{
if ((endingDirection + startingDirection).LengthSquared == 0)
{
endingDirection += new Vector3(.0000001, 0, 0);
}
this.xyz = Vector3Ex.Cross(endingDirection, startingDirection);
this.w = Math.Sqrt(Math.Pow(endingDirection.Length, 2) * Math.Pow(startingDirection.Length, 2)) + Vector3Ex.Dot(endingDirection, startingDirection);
Normalize();
}
public static Vector3 GetPerpendicular(Vector3 a, Vector3 b)
{
if (!Collinear(a, b, Zero))
{
return(a.Cross(b));
}
else
{
Vector3 zOne = new Vector3(0, 0, 100000);
if (!Collinear(a, b, zOne))
{
return(Vector3Ex.Cross(a - zOne, b - zOne));
}
else
{
Vector3 xOne = new Vector3(1000000, 0, 0);
return(Vector3Ex.Cross(a - xOne, b - xOne));
}
}
}
public Plane(Vector3 point0, Vector3 point1, Vector3 point2)
{
this.Normal = Vector3Ex.Cross((point1 - point0), (point2 - point0)).GetNormal();
this.DistanceFromOrigin = Vector3Ex.Dot(Normal, point0);
}
/// <summary>
/// Multiplies two instances.
/// </summary>
/// <param name="left">The first instance.</param>
/// <param name="right">The second instance.</param>
/// <param name="result">A new instance containing the result of the calculation.</param>
public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3Ex.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3Ex.Dot(left.Xyz, right.Xyz));
}
public static Quaternion Mult(Quaternion left, Quaternion right)
{
return(new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3Ex.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3Ex.Dot(left.Xyz, right.Xyz)));
}
