public Pose(Pose linkpose)
{
Position = (double[])linkpose.Position.Clone();
Rotation = (double[])linkpose.Rotation.Clone();
//axisAngle { get; private set; }
this.matrix = (Inventor.Matrix)linkpose.matrix;
internalprecision = linkpose.internalprecision;
}
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);
}
public void GetPosRelativeTo(Pose pose)
{
int i;
for (i = 0; i < 3; i++)
{
this.Position[i] = this.Position[i] - pose.Position[i];
}
if (pose.matrix != null)
{
Inventor.Matrix M = pose.matrix;
Matrix <double> M1 = this.niceMatrix(M);
Vector <double> vec = M1.Multiply(DenseVector.OfArray(this.Position));
this.Position = vec.ToArray();
}
}
//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);
}
public void MirrorPart()
{
Inventor.AssemblyDocument AssemblyDocument;
Inventor.ComponentDefinition ComponentDefinition;
Inventor.ComponentOccurrence ComponentOccurrence;
Inventor.Document Document = InvApp.ActiveDocument;
Inventor.PartDocument PartDocument;
if (Document.DocumentType == Inventor.DocumentTypeEnum.kAssemblyDocumentObject)
{
AssemblyDocument = (Inventor.AssemblyDocument)Document;
Inventor.WorkPlane WorkPlane;
WorkPlane = AssemblyDocument.ComponentDefinition.WorkPlanes["XZ Plane"];
double XNormal;
double YNormal;
double ZNormal;
Inventor.Plane Plane;
Plane = WorkPlane.Plane;
//Get the Normals
XNormal = Plane.Normal.X;
YNormal = Plane.Normal.Y;
ZNormal = Plane.Normal.Z;
//Mirroring matrix
Inventor.Matrix MirroringMatrix = InvApp.TransientGeometry.CreateMatrix();
Inventor.Matrix MMatrix = InvApp.TransientGeometry.CreateMatrix();
double[] MirroringMatrixData = new double[16];
MirroringMatrixData[0] = 1 - 2 * XNormal * XNormal;
MirroringMatrixData[1] = -2 * XNormal * YNormal;
MirroringMatrixData[2] = -2 * XNormal * ZNormal;
MirroringMatrixData[3] = 0;
MirroringMatrixData[4] = -2 * XNormal * YNormal;
MirroringMatrixData[5] = 1 - 2 * YNormal * YNormal;
MirroringMatrixData[6] = -2 * ZNormal * YNormal;
MirroringMatrixData[7] = 0;
MirroringMatrixData[8] = -2 * XNormal * ZNormal;
MirroringMatrixData[9] = -2 * ZNormal * YNormal;
MirroringMatrixData[10] = 1 - 2 * ZNormal * ZNormal;
MirroringMatrixData[11] = 0;
MirroringMatrixData[12] = 0;
MirroringMatrixData[13] = 0;
MirroringMatrixData[14] = 0;
MirroringMatrixData[15] = 1;
//Hand over the marix Data
MirroringMatrix.PutMatrixData(ref MirroringMatrixData);
ComponentOccurrence = AssemblyDocument.ComponentDefinition.Occurrences[1];
ComponentDefinition = ComponentOccurrence.Definition;
MirroringMatrix.PostMultiplyBy(ComponentOccurrence.Transformation);
AssemblyDocument.ComponentDefinition.Occurrences.AddByComponentDefinition(ComponentDefinition, MirroringMatrix);
}
else if (Document.DocumentType == Inventor.DocumentTypeEnum.kPartDocumentObject)
{
PartDocument = (Inventor.PartDocument)Document;
}
}
