public static Matrix3D GetMatrix(Quaternion q) { double n1 = 2 * q.Y * q.Y; double n2 = 2 * q.Z * q.Z; double n3 = 2 * q.X * q.X; double n4 = 2 * q.X * q.Y; double n5 = 2 * q.W * q.Z; double n6 = 2 * q.X * q.Z; double n7 = 2 * q.W * q.Y; double n8 = 2 * q.Y * q.Z; double n9 = 2 * q.W * q.X; Matrix3D result = Matrix3D.Identity; result.M11 = 1 - n1 - n2; result.M12 = n4 + n5; result.M13 = n6 - n7; result.M21 = n4 - n5; result.M22 = 1 - n3 - n2; result.M23 = n8 + n9; result.M31 = n6 + n7; result.M32 = n8 - n9; result.M33 = 1 - n3 - n1; result.M44 = 1; return result; }
/// <summary> /// Calculates the updated offsets for a given frame /// </summary> /// <param name="frame"></param> public void SetFrame(double[] frame, Quaternion prevRotation) { int childCount = Children.Count; Quaternion result = prevRotation; /*Matrix3D matrix = QuaternionRotation3D.GetMatrix(result); matrix.TransformTo(OriginalOffset, Offset);*/ result.RotateTo(OriginalOffset, Offset); if (childCount != 0) { Quaternion rotX = Quaternion.XRotation(frame[XRotationIndex]); Quaternion rotY = Quaternion.YRotation(frame[YRotationIndex]); Quaternion rotZ = Quaternion.ZRotation(frame[ZRotationIndex]); /*if (Name == "RightKnee") { rotX = Quaternion.Identity; rotY = Quaternion.Identity; rotZ = Quaternion.Identity; }*/ result = prevRotation * rotZ * rotX * rotY; } /* if (Parent != null) { Quaternion rotX = Quaternion.XRotation(frame[Parent.XRotationIndex]); Quaternion rotY = Quaternion.YRotation(frame[Parent.YRotationIndex]); Quaternion rotZ = Quaternion.ZRotation(frame[Parent.ZRotationIndex]); result = prevRotation * rotZ * rotX * rotY; } else { result = prevRotation; }*/ if (Parent == null) { // if root node StartPointWorld.X = frame[0] + Offset.X; StartPointWorld.Y = frame[1] + Offset.Y; StartPointWorld.Z = frame[2] + Offset.Z; } else { StartPointWorld.X = Parent.StartPointWorld.X + Offset.X; StartPointWorld.Y = Parent.StartPointWorld.Y + Offset.Y; StartPointWorld.Z = Parent.StartPointWorld.Z + Offset.Z; } for (int i = 0; i < childCount; i++) { Children[i].SetFrame(frame, result); } }
public static Quaternion ZRotation(double angleInRadians) { double halfAngleInRadians = (angleInRadians * 0.5); Quaternion q = new Quaternion(0, 0, Math.Sin(halfAngleInRadians), Math.Cos(halfAngleInRadians)); q.value = Matrix3D.Identity; q.valueIsDirty = true; return q; }
public static Quaternion Subtract(Quaternion left, Quaternion right) { return left - right; }
public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath) { //For more information on how this works see: //Chapter 10 in "Essential Mathermatics for Games & Interactive Applications" //James M. Van Verth & Lars M. Bishop //Slerp(p,q,t) = (sin((1-t)angle) * p + sin(t*angle) * q) / sin(angle) //Interpolate along a sphere between the two quaternions //Find the angle between the two axis double fromNorm = from.Norm; double toNorm = to.Norm; from.Normalize(); to.Normalize(); double fromDotTo = from.X * to.X + from.Y * to.Y + from.Z * to.Z + from.W * to.W; if (useShortestPath) { if (fromDotTo < 0) { //Need to negate one of the quaternions to = new Quaternion(-to.X, -to.Y, -to.Z, -to.W); fromDotTo *= -1; } } if (fromDotTo < -1) { fromDotTo = -1; } else if (fromDotTo > 1) { fromDotTo = 1; } double angleInRadians = System.Math.Acos(fromDotTo); double sinAngle = System.Math.Sin(angleInRadians); //Be careful about division by zero if (MathHelper.IsZero(sinAngle)) { //Just lerp instead of slerp return new Quaternion(from.X + (to.X - from.X) * t, from.Y + (to.Y - from.Y) * t, from.Z + (to.Z - from.Z) * t, from.W + (to.W - from.W) * t); } else { double sinFrom = System.Math.Sin((1.0 - t) * angleInRadians) / sinAngle; double sinTo = System.Math.Sin(t * angleInRadians) / sinAngle; //Scale from FromNorm to toNorm depending on t. When t==0 then //scale == fromNorm, when t==1 scale == toNorm double scale = 1;//fromNorm * System.Math.Pow((toNorm / fromNorm), t); return new Quaternion(scale * (from.X * sinFrom + to.X * sinTo), scale * (from.Y * sinFrom + to.Y * sinTo), scale * (from.Z * sinFrom + to.Z * sinTo), scale * (from.W * sinFrom + to.W * sinTo)); } }
public static Quaternion Slerp(Quaternion from, Quaternion to, double t) { return Slerp(from, to, t, true); }
public static Quaternion Multiply(Quaternion left, Quaternion right) { return left * right; }
static Quaternion() { Quaternion.identity = new Quaternion(0, 0, 0, 1); }
public static Quaternion Add(Quaternion left, Quaternion right) { return left + right; }
public QuaternionRotation3D(Quaternion quaternion) { this.Quaternion = quaternion; }