public static void addRTtoTransform(TransformRT tfm, Transform transform)
{
transform.setTransform(tfm.R.Array[0][0], tfm.R.Array[0][1], tfm.R.Array[0][2],
tfm.R.Array[1][0], tfm.R.Array[1][1], tfm.R.Array[1][2],
tfm.R.Array[2][0], tfm.R.Array[2][1], tfm.R.Array[2][2],
tfm.T.Array[0][0], tfm.T.Array[0][1], tfm.T.Array[0][2]);
}
public static TransformRT[] parseACSFileToRT(string filename)
{
double[][] dat = parseACSFile(filename);
TransformRT[] ACS = new TransformRT[1];
ACS[0] = new TransformRT();
double[][] tempR = { dat[0], dat[1], dat[2] };
ACS[0].R = new GeneralMatrix(tempR, 3, 3);
ACS[0].T = new GeneralMatrix(dat[3], 1);
return(ACS);
}
public static TransformRT[] parseMotionFileToRT(string filename)
{
double[][] dat = parseMotionFile(filename);
TransformRT[] transforms = new TransformRT[15];
for (int i = 0; i < 15; i++)
{
transforms[i] = new TransformRT();
double[][] tempR = { dat[i * 4], dat[i * 4 + 1], dat[i * 4 + 2] };
transforms[i].R = new GeneralMatrix(tempR, 3, 3);
transforms[i].T = new GeneralMatrix(dat[i * 4 + 3], 1);
}
return(transforms);
}
public static TransformRT[] parseInertiaFileToRT(string filename)
{
double[][] dat = parseInertiaFile(filename);
TransformRT[] inertias = new TransformRT[15];
for (int i = 0; i < 15; i++)
{
inertias[i] = new TransformRT();
double[][] tempR = { dat[i * 5 + 2], dat[i * 5 + 3], dat[i * 5 + 4] };
inertias[i].R = new GeneralMatrix(tempR, 3, 3);
inertias[i].T = new GeneralMatrix(dat[i * 5], 1);
}
return(inertias);
}
public static TransformRT[] parseRTFileWithHeaderToRT(string filename)
{
double[][] dat = parseRTFileWithHeaderToDouble(filename);
int numTransforms = (int)dat.Length / 4;
TransformRT[] transforms = new TransformRT[numTransforms];
for (int i = 0; i < numTransforms; i++)
{
transforms[i] = new TransformRT();
double[][] tempR = { dat[i * 4], dat[i * 4 + 1], dat[i * 4 + 2] };
transforms[i].R = new GeneralMatrix(tempR, 3, 3);
transforms[i].T = new GeneralMatrix(dat[i * 4 + 3], 1);
}
return(transforms);
}
public TransformMatrix(TransformRT rt) : base(4, 4)
{
this.setToIdentity();
this.SetMatrix(0, 2, 0, 2, rt.R); // set R
this.SetMatrix(0, 2, 3, 3, rt.T.Transpose()); //set T, do I need to transpose first?
}