public void SetToPosition(TransformMatrix tm)
{
if (_bone.hasTransform())
{
_bone.removeTransform();
}
if (tm.isIdentity())
{
return; //nothing to do in this case
}
else
{
_bone.addTransform(tm.ToTransform());
}
}
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 TransformMatrix CalculateRelativeMotionFromNeutral(int positionIndex, Bone fixedBone)
{
//check for a neutral position
if (positionIndex == 0)
{
return(new TransformMatrix()); //no motion in neutral
}
//check for the fixed bone being us :)
if (this == fixedBone)
{
//TODO: What do I do in this case...?
return(new TransformMatrix());
}
TransformMatrix tmFixedBoneInverse = fixedBone._transformMatrices[positionIndex].Inverse();
return(tmFixedBoneInverse * _transformMatrices[positionIndex]);
}
public void createFromQuaternions(double q0, double q1, double q2, double q3, double[] center)
{
if (center.Length != 3)
{
throw new ArgumentException("Center point must have 3 values");
}
TransformMatrix rotMat = new TransformMatrix();
TransformMatrix transMat = new TransformMatrix();
TransformMatrix transMatInverse = new TransformMatrix();
rotMat.quaternionToRotationMatrix(q0, q1, q2, q3);
transMat.setTranslation(center);
transMatInverse.setTranslation(-center[0], -center[1], -center[2]);
TransformMatrix final = transMat * rotMat * transMatInverse;
SetMatrix(final);
}
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 TransformMatrix[][] parseXrommKinematicFileToTransformMatrix(string filename)
{
double[][] dat = parseDatFile(filename);
if (dat.Length == 0 || dat[0].Length == 0)
{
return(null);
}
if (dat[0].Length % 16 != 0)
{
throw new ArgumentException("Invalid Xromm data file format. Column number must be multiple of 16!");
}
int numCol = dat[0].Length;
int numBones = dat[0].Length / 16;
TransformMatrix[][] transforms = new TransformMatrix[numBones][];
for (int i = 0; i < numBones; i++)
{
transforms[i] = new TransformMatrix[dat.Length];
}
for (int i = 0; i < dat.Length; i++)
{
if (dat[i].Length != numCol)
{
throw new ArgumentException("Invalid XROMM data file format. Every row must have the same number of columns");
}
for (int j = 0; j < numBones; j++)
{
if (Double.IsNaN(dat[i][j * 16]))
{
//what do we do for NaN's.... for now we leave blank...yes?
//so do nothing
}
else
{
transforms[j][i] = new TransformMatrix(dat[i], 16 * j);
}
}
}
return(transforms);
}
public static Switch CreateAnimationSwitch(Bone bone, Bone fixedBone, int[] animationOrder, int numFrames)
{
Switch sw = new Switch();
sw.reference();
//add starting position, each animation does the animation and the ending frame, not the starting one :)
TransformMatrix startPosition = bone.CalculateRelativeMotionFromNeutral(animationOrder[0], fixedBone);
sw.addChild(startPosition.ToTransform());
for (int i = 0; i < animationOrder.Length - 1; i++) //not the last animation, there need start and end
{
TransformMatrix[] tmTransforms = bone.CalculateInterpolatedMotion(animationOrder[i], animationOrder[i + 1], fixedBone, numFrames);
foreach (TransformMatrix tform in tmTransforms)
{
sw.addChild(tform.ToTransform());
}
}
sw.unrefNoDelete();
return(sw);
}
/// <summary>
/// Compares two TransformMatrices to see if they are equal
/// </summary>
/// <param name="B">Matrix to test against</param>
/// <returns></returns>
public bool isEqual(TransformMatrix B)
{
return(isEqual(B, 0.0001f));
}
/// <summary>Linear algebraic matrix multiplication, A * B</summary>
/// <param name="B"> another matrix
/// </param>
/// <returns> Matrix product, A * B
/// </returns>
/// <exception cref="System.ArgumentException"> Matrix inner dimensions must agree.
/// </exception>
public virtual TransformMatrix Multiply(TransformMatrix B)
{
return(new TransformMatrix(base.Multiply(B)));
}
public void SetTransformation(TransformMatrix transform, int positionIndex)
{
_transformMatrices[positionIndex] = transform;
}
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 static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia, TransformMatrix MotionRelativeBone, TransformMatrix MotionTestBone)
{
TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;
return(CalculatePosture(ACS, Inertia, relativeMotion));
}
public static Posture CalculatePosture(TransformMatrix ACS, TransformMatrix Inertia)
{
return(CalculatePosture(ACS, Inertia, new TransformMatrix()));
}
public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS)
{
return(CalculatePronationSupination(RCS, UCS, new TransformMatrix()));
}
public static PronationSupination CalculatePronationSupination(TransformMatrix RCS, TransformMatrix UCS, TransformMatrix MotionTestBone, TransformMatrix MotionRelativeBone)
{
TransformMatrix relativeMotion = MotionRelativeBone.Inverse() * MotionTestBone;
return(CalculatePronationSupination(RCS, UCS, relativeMotion));
}
private TransformMatrix readSingleTransform(StreamReader filestream)
{
const string centerRotationRegex = @"^\s*center\ of\ rotation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string rotationAnglesRegex = @"^\s*rotation\ angles\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string translationRegex = @"^\s*translation\ value\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
const string translationBothRegex = @"^\s*translation\:\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s+([-\d\.e+]+)\s*$";
//lets figure out the transform type
TransformMatrix rt = new TransformMatrix();
string transformLine = filestream.ReadLine();
string transformType = Regex.Match(transformLine, @"^Transform\ type\:\ (.*)$").Groups[1].Value.Trim();
switch (transformType)
{
case "default rotation":
_lastTransformWasOptimizedBoth = false;
double[] centerRotation = new double[3];
double[] rotationAngles = new double[3];
string centRotationLine = filestream.ReadLine();
Match m = Regex.Match(centRotationLine, centerRotationRegex);
centerRotation[0] = Double.Parse(m.Groups[1].Value);
centerRotation[1] = Double.Parse(m.Groups[2].Value);
centerRotation[2] = Double.Parse(m.Groups[3].Value);
string rotationAnglesLines = filestream.ReadLine();
m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
rotationAngles[0] = Double.Parse(m.Groups[1].Value);
rotationAngles[1] = Double.Parse(m.Groups[2].Value);
rotationAngles[2] = Double.Parse(m.Groups[3].Value);
rt.rotateAboutCenter(rotationAngles, centerRotation);
break;
case "default translation":
_lastTransformWasOptimizedBoth = false;
double[] translation = new double[3];
string translationLine = filestream.ReadLine();
m = Regex.Match(translationLine, translationRegex);
translation[0] = Double.Parse(m.Groups[1].Value);
translation[1] = Double.Parse(m.Groups[2].Value);
translation[2] = Double.Parse(m.Groups[3].Value);
rt.setTranslation(translation);
break;
case "optimized both":
_lastTransformWasOptimizedBoth = true;
centerRotation = new double[3];
rotationAngles = new double[3];
translation = new double[3];
centRotationLine = filestream.ReadLine();
m = Regex.Match(centRotationLine, centerRotationRegex);
centerRotation[0] = Double.Parse(m.Groups[1].Value);
centerRotation[1] = Double.Parse(m.Groups[2].Value);
centerRotation[2] = Double.Parse(m.Groups[3].Value);
rotationAnglesLines = filestream.ReadLine();
m = Regex.Match(rotationAnglesLines, rotationAnglesRegex);
rotationAngles[0] = Double.Parse(m.Groups[1].Value);
rotationAngles[1] = Double.Parse(m.Groups[2].Value);
rotationAngles[2] = Double.Parse(m.Groups[3].Value);
translationLine = filestream.ReadLine();
m = Regex.Match(translationLine, translationBothRegex);
translation[0] = Double.Parse(m.Groups[1].Value);
translation[1] = Double.Parse(m.Groups[2].Value);
translation[2] = Double.Parse(m.Groups[3].Value);
TransformMatrix rot = new TransformMatrix();
TransformMatrix t = new TransformMatrix();
rot.rotateAboutCenter(rotationAngles, centerRotation);
t.setTranslation(translation);
rt = t * rot;
_currentOptimizedBothTransform = new EuclideanTransform();
_currentOptimizedBothTransform.CenterRotation = centerRotation;
_currentOptimizedBothTransform.Rotation = rotationAngles;
_currentOptimizedBothTransform.Translation = translation;
break;
default:
//error, there should be no other type
throw new FormatException("Invalid format for transform file. Unknown transform type: " + transformType);
}
return(rt);
}
