コード例 #1
0
 public static double Determinant(Matrix3 matrix)
 {
     float[,] m = matrix.matrix;
     double deter = 0;
     deter = m[0, 0] * m[1, 1] * m[2, 2] + m[0, 1] * m[1, 2] * m[2, 1] +
         m[0, 2] * m[1, 0] * m[2, 1] - m[0, 0] * m[1, 2] * m[2, 1]
         - m[0, 1] * m[1, 0] * m[2, 2] - m[0, 2] * m[1, 1] * m[2, 0];
     return deter;
 }
コード例 #2
0
        /// <summary>
        /// Calculate hip angle
        /// [0]: calculates the (x,y) angle
        /// [1]: calculates the (y,z) angle
        /// [2]: calculates the (x,z) angle
        /// </summary>
        /// <returns>double[]</returns>
        public double[] rotationAngles(JointType currentJoint)
        {
            double[] angles = new double[4];
            if (currentJoint == JointType.ElbowLeft || currentJoint == JointType.ElbowRight
                || currentJoint == JointType.KneeLeft || currentJoint == JointType.KneeRight)
            {
                angles[1] = Vector3.GetAngle(vectNext, vectPrevious);
                return angles;
            }

            SkeletonPoint[] _obj = (SkeletonPoint[])jointLinks[currentJoint];
            SkeletonPoint zAxis = new SkeletonPoint();

            if (_obj.Length == 5)
            {
                zAxis = (SkeletonPoint)(_obj[4]);//the axis for measuring the rotation around the y-axis
            }

            double angle;
            Matrix3 rotationMatrixY = new Matrix3();
            if (_obj.Length == 5)
            {
                angle = angles[3] = Vector2.signedAngle(new Vector2(0, 1), new Vector2(zAxis.X, zAxis.Z));
                rotationMatrixY = RotationMatrix.GetRotationMatrix3Y(angle * Math.PI / 180);
            }
            //find the rotation according to the vertical axis
            angle = Vector2.signedAngle(new Vector2(1, 0), new Vector2(vectPrevious.Y, vectPrevious.Z));
            Matrix3 rotationMatrixX = RotationMatrix.GetRotationMatrix3X(angle * Math.PI / 180);

            angle = Vector2.signedAngle(new Vector2(0, 1), new Vector2(vectPrevious.X, vectPrevious.Y));
            Matrix3 rotationMatrixZ = RotationMatrix.GetRotationMatrix3Z(angle * Math.PI / 180);

            if (_obj.Length == 5)
            {
                vectNext = rotationMatrixY * vectNext;
            }
            vectNext = (rotationMatrixZ * (rotationMatrixX * vectNext));

            angles[0] = Vector2.signedAngle(new Vector2(0, 1), new Vector2(vectNext.X, vectNext.Y));
            angles[1] = Vector2.signedAngle(new Vector2(1, 0), new Vector2(vectNext.Y, vectNext.Z));
            angles[2] = Vector2.signedAngle(new Vector2(0, 1), new Vector2(vectNext.X, vectNext.Z));

            counter++;

            return angles;
        }