public static RotationMatrix3D FromEulerZYZ(double x, double y, double z)
{
var rotationMatrix3D = new RotationMatrix3D();
var num = Math.Cos(x*3.1415926535897931/180.0);
var num2 = Math.Cos(y*3.1415926535897931/180.0);
var num3 = Math.Cos(z*3.1415926535897931/180.0);
var num4 = Math.Sin(x*3.1415926535897931/180.0);
var num5 = Math.Sin(y*3.1415926535897931/180.0);
var num6 = Math.Sin(z*3.1415926535897931/180.0);
rotationMatrix3D[0, 0] = num*num2*num3 - num4*num6;
rotationMatrix3D[0, 1] = -num*num2*num6 - num4*num3;
rotationMatrix3D[0, 2] = num*num5;
rotationMatrix3D[1, 0] = num4*num2*num3 + num*num6;
rotationMatrix3D[1, 1] = -num4*num2*num6 + num*num3;
rotationMatrix3D[1, 2] = num4*num5;
rotationMatrix3D[2, 0] = -num5*num3;
rotationMatrix3D[2, 1] = num5*num6;
rotationMatrix3D[2, 2] = num2;
return rotationMatrix3D;
}
private static RotationMatrix3D RotateZ(double angle)
{
angle *= 0.017453292519943295;
var rotationMatrix3D = new RotationMatrix3D();
rotationMatrix3D[0, 0] = Math.Cos(angle);
rotationMatrix3D[0, 1] = -Math.Sin(angle);
rotationMatrix3D[1, 0] = Math.Sin(angle);
rotationMatrix3D[1, 1] = Math.Cos(angle);
rotationMatrix3D[2, 2] = 1.0;
return rotationMatrix3D;
}
public void Convert()
{
if (!(InputItems == null | OutputItems == null))
{
if (!_isConverting)
{
_isConverting = true;
var result = new Vector3D();
var num = 0.0;
var quaternion = new Quaternion();
switch (InputItems.SelectedItem)
{
case CartesianEnum.ABB_Quaternion:
quaternion = new Quaternion(InputItems.V1, InputItems.V2, InputItems.V3, InputItems.V4);
break;
case CartesianEnum.Roll_Pitch_Yaw:
_rotationMatrix = RotationMatrix3D.FromRPY(InputItems.V1, InputItems.V2, InputItems.V3);
break;
case CartesianEnum.Axis_Angle:
_rotationMatrix =
RotationMatrix3D.RotateAroundVector(
new Vector3D(InputItems.V1, InputItems.V2, InputItems.V3), InputItems.V4);
break;
case CartesianEnum.Kuka_ABC:
_rotationMatrix = RotationMatrix3D.FromABC(InputItems.V1, InputItems.V2, InputItems.V3);
break;
case CartesianEnum.Euler_ZYZ:
_rotationMatrix = RotationMatrix3D.FromEulerZYZ(InputItems.V1, InputItems.V2, InputItems.V3);
break;
}
switch (OutputItems.SelectedItem)
{
case CartesianEnum.ABB_Quaternion:
quaternion = (Quaternion) _rotationMatrix;
result = quaternion.Vector;
num = quaternion.Scalar;
break;
case CartesianEnum.Roll_Pitch_Yaw:
result = _rotationMatrix.RPY;
break;
case CartesianEnum.Axis_Angle:
result = _rotationMatrix.RotationAxis();
num = _rotationMatrix.RotationAngle();
break;
case CartesianEnum.Kuka_ABC:
result = _rotationMatrix.ABC;
break;
case CartesianEnum.Euler_ZYZ:
result = _rotationMatrix.EulerZYZ;
break;
case CartesianEnum.Alpha_Beta_Gamma:
result = _rotationMatrix.ABG;
break;
}
var text = quaternion.ToString();
// var text = quaternion.ToString("F3");
if (Matrix != null && Matrix != text)
{
Matrix = text;
}
WriteValues(result, 0.0, false);
if (OutputItems.SelectedItem == CartesianEnum.ABB_Quaternion)
{
WriteValues(result, num, true);
}
if (OutputItems.SelectedItem == CartesianEnum.Axis_Angle)
{
OutputItems.V4 = num;
}
_isConverting = false;
}
}
}