/// <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, double blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0)
{
if (q2.LengthSquared == 0.0)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == 0.0)
{
return(q1);
}
double cosHalfAngle = q1.W * q2.W + Vector3Ex.Dot(q1.Xyz, q2.Xyz);
if (cosHalfAngle >= 1.0 || cosHalfAngle <= -1.0)
{
// angle = 0.0, so just return one input.
return(q1);
}
else if (cosHalfAngle < 0.0)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
double blendA;
double blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
double halfAngle = (double)System.Math.Acos(cosHalfAngle);
double sinHalfAngle = (double)System.Math.Sin(halfAngle);
double oneOverSinHalfAngle = 1.0 / sinHalfAngle;
blendA = (double)System.Math.Sin(halfAngle * (1.0 - blend)) * oneOverSinHalfAngle;
blendB = (double)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0 - blend;
blendB = blend;
}
Quaternion result = new Quaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}
public bool RayHitPlane(Ray ray, out double distanceToHit, out bool hitFrontOfPlane)
{
distanceToHit = double.PositiveInfinity;
hitFrontOfPlane = false;
double normalDotRayDirection = Vector3Ex.Dot(Normal, ray.directionNormal);
if (normalDotRayDirection < TreatAsZero && normalDotRayDirection > -TreatAsZero) // the ray is parallel to the plane
{
return(false);
}
if (normalDotRayDirection < 0)
{
hitFrontOfPlane = true;
}
double distanceToRayOriginFromOrigin = Vector3Ex.Dot(Normal, ray.origin);
double distanceToPlaneFromRayOrigin = DistanceFromOrigin - distanceToRayOriginFromOrigin;
bool originInFrontOfPlane = distanceToPlaneFromRayOrigin < 0;
bool originAndHitAreOnSameSide = originInFrontOfPlane == hitFrontOfPlane;
if (!originAndHitAreOnSameSide)
{
return(false);
}
distanceToHit = distanceToPlaneFromRayOrigin / normalDotRayDirection;
return(true);
}
public double GetDistanceToIntersection(Vector3 pointOnLine, Vector3 lineDirection)
{
double normalDotRayDirection = Vector3Ex.Dot(Normal, lineDirection);
if (normalDotRayDirection < TreatAsZero && normalDotRayDirection > -TreatAsZero) // the ray is parallel to the plane
{
return(double.PositiveInfinity);
}
double planeNormalDotPointOnLine = Vector3Ex.Dot(Normal, pointOnLine);
return((DistanceFromOrigin - planeNormalDotPointOnLine) / normalDotRayDirection);
}
public static Plane Transform(Plane inputPlane, Matrix4X4 matrix)
{
Vector3 planeNormal = inputPlane.Normal;
double distanceToPlane = inputPlane.DistanceFromOrigin;
Plane outputPlane = new Plane();
outputPlane.Normal = Vector3Ex.TransformVector(planeNormal, matrix).GetNormal();
Vector3 pointOnPlane = planeNormal * distanceToPlane;
Vector3 pointOnTransformedPlane = Vector3Ex.Transform(pointOnPlane, matrix);
outputPlane.DistanceFromOrigin = Vector3Ex.Dot(outputPlane.Normal, pointOnTransformedPlane);
return(outputPlane);
}
public double GetDistanceToIntersection(Ray ray, out bool inFront)
{
inFront = false;
double normalDotRayDirection = Vector3Ex.Dot(Normal, ray.directionNormal);
if (normalDotRayDirection < TreatAsZero && normalDotRayDirection > -TreatAsZero) // the ray is parallel to the plane
{
return(double.PositiveInfinity);
}
if (normalDotRayDirection < 0)
{
inFront = true;
}
return((DistanceFromOrigin - Vector3Ex.Dot(Normal, ray.origin)) / normalDotRayDirection);
}
public bool LineHitPlane(Vector3 start, Vector3 end, out Vector3 intersectionPosition)
{
double distanceToStartFromOrigin = Vector3Ex.Dot(Normal, start);
if (distanceToStartFromOrigin == 0)
{
intersectionPosition = start;
return(true);
}
double distanceToEndFromOrigin = Vector3Ex.Dot(Normal, end);
if (distanceToEndFromOrigin == 0)
{
intersectionPosition = end;
return(true);
}
if ((distanceToStartFromOrigin < 0 && distanceToEndFromOrigin > 0) ||
(distanceToStartFromOrigin > 0 && distanceToEndFromOrigin < 0))
{
Vector3 direction = (end - start).GetNormal();
double startDistanceFromPlane = distanceToStartFromOrigin - DistanceFromOrigin;
double endDistanceFromPlane = distanceToEndFromOrigin - DistanceFromOrigin;
double lengthAlongPlanNormal = endDistanceFromPlane - startDistanceFromPlane;
double ratioToPlanFromStart = startDistanceFromPlane / lengthAlongPlanNormal;
intersectionPosition = start + direction * ratioToPlanFromStart;
return(true);
}
intersectionPosition = Vector3.PositiveInfinity;
return(false);
}
public Plane(Vector3 planeNormal, Vector3 pointOnPlane)
{
this.Normal = planeNormal.GetNormal();
this.DistanceFromOrigin = Vector3Ex.Dot(planeNormal, pointOnPlane);
}
public Plane(Vector3 point0, Vector3 point1, Vector3 point2)
{
this.Normal = Vector3Ex.Cross((point1 - point0), (point2 - point0)).GetNormal();
this.DistanceFromOrigin = Vector3Ex.Dot(Normal, point0);
}
public double GetDistanceFromPlane(Vector3 positionToCheck)
{
double distanceToPosition = Vector3Ex.Dot(Normal, positionToCheck);
return(distanceToPosition - DistanceFromOrigin);
}
/// <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();
}
/// <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)));
}
