public double[] transform(double[] displacement, bool isreverse)
{
DenseMatrix xform = (isreverse ? (DenseMatrix)transformation.Inverse() : transformation);
double[] dispcopy = new double[6];
displacement.CopyTo(dispcopy, 0);
DenseMatrix2String m2s = new DenseMatrix2String();
List2String l2s = new List2String();
DenseVector dispV = new DenseVector(dispcopy);
log.Debug("original disp: " + l2s.ToString(displacement));
if (isreverse)
{
dispV = (DenseVector)xform.Multiply(dispV);
log.Debug("transformed Disp: " + l2s.ToString(dispV.Values));
}
DenseVector newDisp = translate(dispV.Values, isreverse);
log.Debug("newDisp: " + l2s.ToString(newDisp.Values));
DenseMatrix rollM = roll.create(dispV.Values[3]);
DenseMatrix pitchM = pitch.create(dispV.Values[4]);
DenseMatrix yawM = yaw.create(dispV.Values[5]);
DenseMatrix rotation = (DenseMatrix)rollM.Multiply(pitchM.Multiply(yawM));
log.Debug("rotation: " + m2s.ToString(rotation));
DenseVector unt = (DenseVector)rotation.Multiply(directionalVector);
log.Debug("unt: " + l2s.ToString(unt.Values));
DenseVector newDisp1 = (DenseVector)unt.Add(newDisp);
log.Debug("newDisp1: " + l2s.ToString(newDisp1.Values));
dispV.SetSubVector(0, 3, newDisp1);
if (isreverse == false)
{
dispV = (DenseVector)xform.Multiply(dispV);
}
log.Debug("resulting Disp: " + l2s.ToString(dispV.Values));
return dispV.Values;
}
public void setLbcb(bool isLbcb2, double[] flattenedPinArray, double[] motionCenter, double[] flattenedTransformation)
{
double[][] pins = new double[6][];
double[,] xform = new double[6, 6];
double[,] xpins = new double[6, 6];
List2String l2s = new List2String();
DenseMatrix2String m2s = new DenseMatrix2String();
for (int r = 0; r < 6; r++)
{
pins[r] = new double[6];
for (int c = 0; c < 6; c++)
{
pins[r][c] = flattenedPinArray[r * 6 + c];
xpins[r,c] = flattenedPinArray[r * 6 + c];
xform[r, c] = flattenedTransformation[r * 6 + c];
}
}
log.Info("Creating " + (isLbcb2 ? "LBCB 2" : "LBCB 1")
+ " with \n]\t Motion Center: " + l2s.ToString(motionCenter)
+ "\n\t Transformation Matrix: " + m2s.ToString(DenseMatrix.OfArray(xform))
+ "\n\t Pins Locations: " + m2s.ToString(DenseMatrix.OfArray(xpins)));
Lbcb lbcb = new Lbcb((isLbcb2 ? "LBCB 2" : "LBCB 1"), pins);
lbcbs[(isLbcb2 ? 1 : 0)] = lbcb;
RigidTransform transform = new RigidTransform(motionCenter,xform);
transforms[(isLbcb2 ? 1 : 0)] = transform;
}