Esempio n. 1
0
 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;
 }
Esempio n. 2
0
 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;
                }
            }
        }