public Quaternion(Quaternion q)
{
X = q.X;
Y = q.Y;
Z = q.Z;
W = q.W;
}
private bool Equals(Quaternion other)
{
return Equals(other);
}
public static Quaternion Subtract(Quaternion q1, Quaternion q2)
{
return new Quaternion
{
Scalar = q1.Scalar - q2.Scalar,
Vector = q1.Vector - q2.Vector
};
}
public static Quaternion Multiply(Quaternion q1, Quaternion q2)
{
var quaternion = new Quaternion();
var vector3D = new Vector3D(q1.X, q1.Y, q1.Z);
var vector3D2 = new Vector3D(q2.X, q2.Y, q2.Z);
var w = q1.W;
var w2 = q2.W;
var vector3D3 = (Vector3D) (w*vector3D2 + w2*vector3D) + Vector3D.Cross(vector3D, vector3D2);
quaternion.X = vector3D3[0];
quaternion.Y = vector3D3[1];
quaternion.Z = vector3D3[2];
quaternion.W = w*w2 - Classes.Vector.Dot(vector3D, vector3D2);
return quaternion;
}
public static Quaternion FromAxisAngle(Vector axis, double angle)
{
angle *= 0.017453292519943295;
axis.Normalise();
var num = Math.Sin(angle/2.0);
var w = Math.Cos(angle/2.0);
var quaternion = new Quaternion(axis[0]*num, axis[1]*num, axis[2]*num, w);
quaternion.Normalise();
return quaternion;
}
public static bool Equals(Quaternion q1, Quaternion q2)
{
return q1 == q2;
}
public static Quaternion Add(Quaternion q1, Quaternion q2)
{
return new Quaternion
{
Scalar = q1.Scalar + q2.Scalar,
Vector = q1.Vector + q2.Vector
};
}
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;
}
}
}