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;
        }
Пример #2
0
        /// <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);
            }
        }
Пример #3
0
 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;
 }
Пример #4
0
 public static Quaternion Subtract(Quaternion left, Quaternion right)
 {
     return left - right;
 }
Пример #5
0
        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));
            }
        }
Пример #6
0
 public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
 {
     return Slerp(from, to, t, true);
 }
Пример #7
0
 public static Quaternion Multiply(Quaternion left, Quaternion right)
 {
     return left * right;
 }
Пример #8
0
 static Quaternion()
 {
     Quaternion.identity = new Quaternion(0, 0, 0, 1);
 }
Пример #9
0
 public static Quaternion Add(Quaternion left, Quaternion right)
 {
     return left + right;
 }
 public QuaternionRotation3D(Quaternion quaternion)
 {
     this.Quaternion = quaternion;
 }