public static Switch CreateHAMSwitch(Bone bone, Bone fixedBone, int[] animationOrder, int numFrames)
{
Switch sw = new Switch();
sw.reference();
//add starting HAM (none shown), each animation does the animation and the ending frame, not the starting one :)
Separator nullSep = new Separator();
sw.addChild(nullSep);
for (int i = 0; i < animationOrder.Length - 1; i++) //not the last animation, there need start and end
{
TransformMatrix tmTransform = bone.CalculateRelativeMotion(animationOrder[i], animationOrder[i + 1], fixedBone);
HelicalTransform tform = tmTransform.ToHelical();
if (bone.HasInertia)
{
double[] cent = { bone.InertiaMatrix.GetElement(0, 3), bone.InertiaMatrix.GetElement(1, 3), bone.InertiaMatrix.GetElement(2, 3) };
tform.AdjustQToLocateHamNearCentroid(cent);
}
HamAxis axis = new HamAxis(tform.N[0], tform.N[1], tform.N[2], tform.Q[0], tform.Q[1], tform.Q[2]);
for (int j = 0; j < numFrames - 1; j++) //do one less then num frames, no HAM shown for the final position
{
sw.addChild(axis);
}
sw.addChild(nullSep); //add the empty at the end :)
}
sw.unrefNoDelete();
return(sw);
}
public TransformMatrix(HelicalTransform ham) : base(4, 4)
{
setToIdentity();
//sourced from Helical_To_RT.m
//original source: Kinzel et al., JOB 5:93-105, 1972
double phi_r = ham.Phi * Math.PI / 180; //convert phi to radians
double vers = 1 - Math.Cos(phi_r);
this.Array[0][0] = ham.N[0] * ham.N[0] * vers + Math.Cos(phi_r);
this.Array[0][1] = ham.N[0] * ham.N[1] * vers - ham.N[2] * Math.Sin(phi_r);
this.Array[0][2] = ham.N[0] * ham.N[2] * vers + ham.N[1] * Math.Sin(phi_r);
this.Array[1][0] = ham.N[0] * ham.N[1] * vers + ham.N[2] * Math.Sin(phi_r);
this.Array[1][1] = ham.N[1] * ham.N[1] * vers + Math.Cos(phi_r);
this.Array[1][2] = ham.N[1] * ham.N[2] * vers - ham.N[0] * Math.Sin(phi_r);
this.Array[2][0] = ham.N[2] * ham.N[0] * vers - ham.N[1] * Math.Sin(phi_r);
this.Array[2][1] = ham.N[1] * ham.N[2] * vers + ham.N[0] * Math.Sin(phi_r);
this.Array[2][2] = ham.N[2] * ham.N[2] * vers + Math.Cos(phi_r);
//Translation
GeneralMatrix R = this.GetMatrix(0, 2, 0, 2);
GeneralMatrix q = new GeneralMatrix(ham.Q, 3);
GeneralMatrix Rq = R * q;
this.Array[0][3] = ham.Q[0] + (ham.N[0] * ham.Trans) - Rq.Array[0][0];
this.Array[1][3] = ham.Q[1] + (ham.N[1] * ham.Trans) - Rq.Array[1][0];
this.Array[2][3] = ham.Q[2] + (ham.N[2] * ham.Trans) - Rq.Array[2][0];
}
public HelicalTransform(HelicalTransform ham)
{
_phi = ham.Phi;
_n = ham.N;
_trans = ham._trans;
_q = ham._q;
}
public HelicalTransform(TransformMatrix matrix)
{
HelicalTransform ham = matrix.ToHelical();
_phi = ham.Phi;
_n = ham.N;
_trans = ham._trans;
_q = ham._q;
}
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);
}
/// <summary>
/// Will linearly interpolate the current transform into a given number of steps. Just
/// splits up phi and T
/// </summary>
/// <param name="numSteps">Number of steps to divide into, must be >=1, if set to 1 gives no interpilation, just returns a copy of the current</param>
/// <returns>Array of stepped transforms, and last == this transform, length of array == numSteps</returns>
public HelicalTransform[] LinearlyInterpolateMotion(int numSteps)
{
if (numSteps < 1)
{
throw new IndexOutOfRangeException("Can not interpolate to fewer then 1 step");
}
HelicalTransform[] steps = new HelicalTransform[numSteps];
for (int i = 0; i < numSteps; i++)
{
steps[i] = new HelicalTransform();
steps[i]._phi = _phi * (i + 1) / numSteps;
steps[i]._n = _n;
steps[i]._trans = _trans * (i + 1) / numSteps;
steps[i]._q = _q;
}
return(steps);
}
