public TransformMatrix CalculateRelativeMotion(int startPositionIndex, int endPositionIndex, Bone fixedBone)
{
TransformMatrix startRelTransform = this.CalculateRelativeMotionFromNeutral(startPositionIndex, fixedBone);
TransformMatrix endRelTransform = this.CalculateRelativeMotionFromNeutral(endPositionIndex, fixedBone);
return(endRelTransform * startRelTransform.Inverse());
}
public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix BoneRelMotion)
{
Posture post = new Posture();
TransformMatrix AnatIner = ACS.Inverse() * BoneRelMotion * Inertia;
double[] myInertia = { 0, 0, 0 };
for (int i = 0; i < 3; i++)
{
myInertia[i] = AnatIner.Array[i][POS_AXIS]; //select which axis to use....
}
double x = PROJ_PLANES_SIGN[0] * myInertia[PROJ_PLANES[0]];
double y = PROJ_PLANES_SIGN[1] * myInertia[PROJ_PLANES[1]];
double z = PROJ_PLANES_SIGN[2] * myInertia[PROJ_PLANES[2]];
CartesianCoordinate coord = cart2spherical(x, y, z);
double theta = coord.az;
double phi = coord.elev;
//if (PROJ_PLANES == {1, 3, -2}...
theta = (theta / Math.Abs(theta)) * Math.PI - theta;
//endif
post.FE_Raw = theta * 180 / Math.PI;
post.RU_Raw = phi * 180 / Math.PI;
post.FE = post.FE_Raw - NEUTRAL_FE_POSITION;
post.RU = post.RU_Raw - NEUTRAL_RU_POSITION;
return(post);
}
public void CalculateAndSaveDistanceMapForAnimation(int[] animationOrder, int numFramesPerStep, int absoluteFrameNumber, Bone[] testBones, Bone fixedBone)
{
//quick check that we can do this. Don't need our own distance field, just the testBones'
if (_coloredBone == null)
{
return;
}
//need to calculate where we are in the whole animation scheme. What two positions are we between?
int startPositionIndex = absoluteFrameNumber / numFramesPerStep;
int partialFrame = absoluteFrameNumber % numFramesPerStep;
int startPosition = animationOrder[startPositionIndex];
//calculate the relative motion for each bone. Need a transform for each testBone, that will
//move this bone into its coordinate system
TransformMatrix[] transforms = new TransformMatrix[testBones.Length];
if (partialFrame == 0) //no partial frame, we are exactly where we want to be
{
if (startPosition != 0) //if we are at absolute neutral, no calculations needed
{
//check for the non
for (int i = 0; i < testBones.Length; i++)
{
transforms[i] = CalculateRelativeMotionFromNeutral(startPosition, testBones[i]);
}
}
}
else
{
//so we have a partial motion to deal with, first calculate the position of the current bone
TransformMatrix tmCurrentBone = CalculateInterpolatedMotion(startPosition, animationOrder[startPositionIndex + 1], fixedBone, numFramesPerStep)[partialFrame];
for (int i = 0; i < testBones.Length; i++)
{
TransformMatrix tmTestBone = testBones[i].CalculateInterpolatedMotion(startPosition, animationOrder[startPositionIndex + 1], fixedBone, numFramesPerStep)[partialFrame];
transforms[i] = tmTestBone.Inverse() * tmCurrentBone;
}
}
double[] distances = DistanceMaps.createDistanceMap(this, testBones, transforms);
lock (this)
{
if (_animationComputedDistances == null)
{
_animationComputedDistances = new double[(animationOrder.Length - 1) * numFramesPerStep + 1][];
}
_animationComputedDistances[absoluteFrameNumber] = distances;
}
}
public TransformMatrix[] CalculateInterpolatedMotion(int startPositionIndex, int endPositionIndex, Bone startFixedBone, Bone endFixedBone, int numSteps)
{
TransformMatrix[] finalTransforms = new TransformMatrix[numSteps];
TransformMatrix startTransform = this.CalculateRelativeMotionFromNeutral(startPositionIndex, startFixedBone);
TransformMatrix endTransform = this.CalculateRelativeMotionFromNeutral(endPositionIndex, endFixedBone);
TransformMatrix relMotion = endTransform * startTransform.Inverse();
HelicalTransform relMotionHT = relMotion.ToHelical();
HelicalTransform[] htTransforms = relMotionHT.LinearlyInterpolateMotion(numSteps);
for (int i = 0; i < numSteps; i++)
{
finalTransforms[i] = htTransforms[i].ToTransformMatrix() * startTransform;
}
return(finalTransforms);
}
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 Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix MotionRelativeBone, TransformMatrix MotionTestBone)
{
TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;
return(CalculatePosture(ACS, Inertia, relativeMotion));
}
public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix MotionTestBone, TransformMatrix MotionRelativeBone)
{
TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;
return(CalculatePronationSupination(RCS, UCS, relativeMotion));
}