示例#1
0
        public libWrist.TransformMatrix getEditedTransform()
        {
            double[] centerRotation = new double[3];
            double[] rotationAngles = new double[3];
            double[] translation    = new double[3];

            centerRotation[0] = (double)numericUpDownCenterX.Value;
            centerRotation[1] = (double)numericUpDownCenterY.Value;
            centerRotation[2] = (double)numericUpDownCenterZ.Value;

            rotationAngles[0] = (double)numericUpDownRotX.Value;
            rotationAngles[1] = (double)numericUpDownRotY.Value;
            rotationAngles[2] = (double)numericUpDownRotZ.Value;

            translation[0] = (double)numericUpDownTransX.Value;
            translation[1] = (double)numericUpDownTransY.Value;
            translation[2] = (double)numericUpDownTransZ.Value;
            libWrist.TransformMatrix rot = new libWrist.TransformMatrix();
            libWrist.TransformMatrix t   = new libWrist.TransformMatrix();
            rot.rotateAboutCenter(rotationAngles, centerRotation);
            t.setTranslation(translation);
            return(t * rot);
        }
示例#2
0
        /// <summary>
        /// ////////////////////
        /// </summary>
        /// <returns></returns>
        public libWrist.TransformMatrix setNewEditedTransform(float x, float y, float z, float q0, float q1, float q2, float q3)
        {
            double[] centerRotation = new double[3];
            double[] rotationAngles = new double[3];
            double[] translation    = new double[3];

            centerRotation[0] = (double)numericUpDownCenterX.Value;
            centerRotation[1] = (double)numericUpDownCenterY.Value;
            centerRotation[2] = (double)numericUpDownCenterZ.Value;

            //use a method specially designed for getting roll, pitch, and yaw
            float[] quat = new float[4];
            quat[0] = q0;
            quat[1] = q1;
            quat[2] = q2;
            quat[3] = q3;
            float[] euler = new float[3];

            quat2euler(quat, euler);
            double roll  = euler[2];
            double pitch = euler[1];
            double yaw   = euler[0];

            //keep small number
            if (roll > 3.1416)
            {
                roll = -Math.PI + (roll - Math.PI);
            }
            if (roll < -3.1416)
            {
                roll = Math.PI + (roll - (-1) * Math.PI);
            }

            if (pitch > 3.1416)
            {
                pitch = -Math.PI + (pitch - Math.PI);
            }
            if (pitch < -3.1416)
            {
                pitch = Math.PI + (pitch - (-1) * 3.1416F);
            }

            if (yaw > 3.1416)
            {
                yaw = -Math.PI + (yaw - Math.PI);
            }
            if (yaw < -3.1416)
            {
                yaw = Math.PI + (yaw - (-1) * Math.PI);
            }

            //test
            numericUpDownRotX.Value = (decimal)yaw;
            numericUpDownRotY.Value = (decimal)pitch;
            numericUpDownRotZ.Value = (decimal)roll;

            rotationAngles[0] = (double)numericUpDownRotX.Value;
            rotationAngles[1] = (double)numericUpDownRotY.Value;
            rotationAngles[2] = (double)numericUpDownRotZ.Value;

            x = (float)numericUpDownTransX.Value + x;
            y = (float)numericUpDownTransY.Value + y;
            z = (float)numericUpDownTransZ.Value + z;

            if (x > 350)
            {
                x = 350;
            }
            if (x < -350)
            {
                x = -350;
            }
            if (y > 350)
            {
                y = 350;
            }
            if (y < -350)
            {
                y = -350;
            }
            if (z > 350)
            {
                z = 350;
            }
            if (z < -350)
            {
                z = -350;
            }

            numericUpDownTransX.Value = (decimal)x;
            numericUpDownTransY.Value = (decimal)y;
            numericUpDownTransZ.Value = (decimal)z;

            translation[0] = (double)numericUpDownTransX.Value;
            translation[1] = (double)numericUpDownTransY.Value;
            translation[2] = (double)numericUpDownTransZ.Value;


            libWrist.TransformMatrix rot = new libWrist.TransformMatrix();
            libWrist.TransformMatrix t   = new libWrist.TransformMatrix();


            rot.rotateAboutCenter(rotationAngles, centerRotation);
            //rotations go in order Z Y X


            t.setTranslation(translation);
            return(t * rot);
        }