public Matrix <double> niceMatrix(Inventor.Matrix M)
{
Matrix <double> M1 = DenseMatrix.OfArray(new double[, ] {
{ M.get_Cell(1, 1), M.get_Cell(2, 1), M.get_Cell(3, 1) },
{ M.get_Cell(1, 2), M.get_Cell(2, 2), M.get_Cell(3, 2) },
{ M.get_Cell(1, 3), M.get_Cell(2, 3), M.get_Cell(3, 3) }
});
return(M1);
}
//If rotation given, we need more info
public Pose(Inventor.Matrix R1, int precision, double positionScale = 1)
{
//Get global position and rotation
Inventor.Vector pos = R1.Translation;
this.internalprecision = precision;
this.matrix = R1;
//Calculate rotation.
double[] Er = new double[3] {
0, 0, 0
};
double cy_thresh = 0.00000004;
double cy = Math.Sqrt(Math.Pow(R1.get_Cell(3, 3), 2) + Math.Pow(R1.get_Cell(3, 2), 2));
if (cy > cy_thresh)
{
Er[2] = Math.Atan2(R1.get_Cell(2, 1), R1.get_Cell(1, 1)); //Z
Er[1] = Math.Atan2(-R1.get_Cell(3, 1), cy); //Y
Er[0] = Math.Atan2(R1.get_Cell(3, 2), R1.get_Cell(3, 3)); //X
}
else
{
Er[2] = Math.Atan2(R1.get_Cell(1, 3), R1.get_Cell(2, 2)); //Z
Er[1] = Math.Atan2(-R1.get_Cell(3, 1), cy); //Y
Er[0] = 0; //X
}
this.Position = new double[3] {
pos.X, pos.Y, pos.Z
};
this.Rotation = Er;
this.Scale(positionScale);
//this.SetAxisAngle(R1);
}