public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix BoneRelMation)
{
TransformMatrix relativeCSMotion = UCS.Inverse() * BoneRelMation * RCS;
double[] euler = relativeCSMotion.ToEuler();
PronationSupination result = new PronationSupination();
result.Euler_x = euler[0];
result.Euler_y = euler[1];
result.Euler_z = euler[2];
double pronationAngle = euler[0] - NEUTRAL_PS_POSITION; //correct for offset
//correct for negative angles, want final wrist position to be [-180 180]
while (pronationAngle < -180)
{
pronationAngle += 360;
}
result.PronationAngle = pronationAngle;
return(result);
}
public static PronationSupination[] CalculatePosturesPS(Bone radius, Bone ulna)
{
//First check if we have enough
if (!radius.IsValidBone || !radius.HasInertia ||
!ulna.IsValidBone || !ulna.HasInertia)
{
return(null);
}
//Lets create an array for the number of postures...
int numPositions = radius.TransformMatrices.Length;
PronationSupination[] postures = new PronationSupination[numPositions];
for (int i = 0; i < numPositions; i++)
{
postures[i] = CalculatePronationSupination(radius.InertiaMatrix, ulna.InertiaMatrix,
radius.TransformMatrices[i], ulna.TransformMatrices[i]);
}
return(postures);
}
