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