/// <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))); }