예제 #1
0
        internal static MyQuat InvertQuaternion(MyQuat q)
        {
            q.x = -q.x;
            q.y = -q.y;
            q.z = -q.z;

            return(q);
        }
예제 #2
0
        public static MyQuat GetTwist(MyQuat rot3)
        {
            float angle1  = 0;
            float angle2  = 0;
            float angle3  = 33;
            float dangle3 = 33;

            float angle1s = 0;
            float angle2s = 0;
            float angle3s = 40;

            float angle1g = -57;
            float angle2g = 3;
            float angle3g = 10;

            if (angle3 > angle3g + .5)
            {
                angle3 = Lerp(angle3s, angle3g, .002f * (TimeSinceMidnight - timeAtStart));
            }


            //todo: change the return value for exercise 3
            MyVec rotA1, rotA2, rotA3, rotA4;

            rotA1.x = 0;
            rotA1.y = 1;
            rotA1.z = 0;

            rotA2.x = 0;
            rotA2.y = 0;
            rotA2.z = 0;

            rotA3.x = 1;
            rotA3.y = 0;
            rotA3.z = 0;

            rotA4.x = 0;
            rotA4.y = 0;
            rotA4.z = 1;

            MyQuat result;

            result = Rotate(rot3, rotA1, angle3);


            return(result);
        }
예제 #3
0
        //EX1: this function will place the robot in the initial position

        public void PutRobotStraight(out MyQuat rot0, out MyQuat rot1, out MyQuat rot2, out MyQuat rot3)
        {
            //todo: change this, use the function Rotate declared below


            MyVec rotA1, rotA2, rotA3, rotA4;

            rotA1.x = 0;
            rotA1.y = 1;
            rotA1.z = 0;

            rotA2.x = 0;
            rotA2.y = 0;
            rotA2.z = 0;

            rotA3.x = 1;
            rotA3.y = 0;
            rotA3.z = 0;

            rotA4.x = 0;
            rotA4.y = 0;
            rotA4.z = 1;

            quati1 = emptyQuaternion(); //0
            quati1 = Rotate(quati1, rotA1, 74);
            rot0   = quati1;

            quati2 = Rotate(rot0, rotA1, 0);
            rot1   = quati2;

            quati3 = emptyQuaternion();
            quati3 = Rotate(rot1, rotA3, 60);
            rot2   = quati3;

            quati4 = emptyQuaternion();
            quati4 = Rotate(rot1, rotA3, 110);
            rot3   = quati4;

            inPosition  = true;
            timeAtStart = TimeSinceMidnight;
        }
예제 #4
0
        internal static MyQuat Rotate(MyQuat currentRotation, MyVec axis, float angle)
        {
            //takes currentRotation, and calculates a new quaternion rotated by an angle "angle" along the normalized axis "axis"

            MyQuat result, quat2;
            MyVec  qV2;

            //Create a second quaternion from the axis and angle:
            float theta = ((float)Math.PI / 180) * angle; //convert euler angle to radians (theta)!!

            quat2.w = (float)Math.Cos(theta / 2);
            qV2     = ScaleVector3(axis, (float)Math.Sin(theta / 2));
            quat2.x = qV2.x;
            quat2.y = qV2.y;
            quat2.z = qV2.z;

            //Multiply both quaternions to implement the rotation:
            result = Multiply(currentRotation, quat2);

            return(result);
        }
예제 #5
0
        internal static MyQuat Multiply(MyQuat q1, MyQuat q2)
        {
            MyQuat result;
            MyVec  qV1, qV2, rV;

            qV1.x = q1.x;
            qV1.y = q1.y;
            qV1.z = q1.z;

            qV2.x = q2.x;
            qV2.y = q2.y;
            qV2.z = q2.z;

            result.w = q1.w * q2.w + Vector3DotProduct(qV1, qV2);
            rV       = AddVector3(AddVector3(ScaleVector3(qV1, q2.w), ScaleVector3(qV2, q1.w)), Vector3CrossProduct(qV1, qV2));

            result.x = rV.x;
            result.y = rV.y;
            result.z = rV.z;

            return(result);
        }
예제 #6
0
 public static MyQuat GetSwing(MyQuat rot3)
 {
     //it's done in the pickstudanimvertical function
     return(rot3);
 }
예제 #7
0
        //EX3: this function will calculate the rotations necessary to move the arm of the robot until its end effector collides with the target (called Stud_target)
        //it will return true until it has reached its destination. The main project is set up in such a way that when the function returns false, the object will be droped and fall following gravity.
        //the only difference wtih exercise 2 is that rot3 has a swing and a twist, where the swing will apply to joint3 and the twist to joint4

        public bool PickStudAnimVertical(out MyQuat rot0, out MyQuat rot1, out MyQuat rot2, out MyQuat rot3)
        {
            MyVec rotA1, rotA2, rotA3, rotA4;

            rotA1.x = 0;
            rotA1.y = 1;
            rotA1.z = 0;

            rotA2.x = 0;
            rotA2.y = 0;
            rotA2.z = 0;

            rotA3.x = 1;
            rotA3.y = 0;
            rotA3.z = 0;

            rotA4.x = 0;
            rotA4.y = 0;
            rotA4.z = 1;

            if (inPosition)
            {
                myCondition = true;
                inPosition  = false;
                //timeAnim1 = TimeSinceMidnight + timeframe;
            }
            //todo: add a check for your condition



            if (myCondition)
            {
                if (angle1 > angle1g - .5)
                {
                    angle1 = Lerp(angle1s, angle1g, .0006f * (TimeSinceMidnight - timeAtStart));
                }
                else
                {
                    arrive1 = true;
                }

                if (angle2 < 30 + .5)
                {
                    angle2 = Lerp(angle2s, 30, .0006f * (TimeSinceMidnight - timeAtStart));
                }
                else
                {
                    arrive2 = true;
                }

                if (angle3 > angle3g - .5)
                {
                    angle3 = Lerp(angle3s, angle3g, .0006f * (TimeSinceMidnight - timeAtStart));
                }
                else
                {
                    arrive3 = true;
                }



                quati9 = emptyQuaternion(); //0
                quati9 = Rotate(quati1, rotA1, angle1);
                rot0   = quati9;

                quati10 = Rotate(rot0, rotA3, angle2);
                rot1    = quati10;

                quati11 = emptyQuaternion();
                quati11 = Rotate(rot0, rotA3, 60);
                rot2    = quati11;

                quati12 = emptyQuaternion();
                quati12 = Rotate(rot0, rotA3, angle3);
                rot3    = quati12;

                if (arrive1 && arrive2 && arrive3)
                {
                    myCondition = false;
                    angle1      = 0;
                    angle2      = 0;
                    angle3      = 110;
                    arrive1     = arrive2 = arrive3 = false;
                }

                return(true);
            }

            rot0 = NullQ;
            rot1 = NullQ;
            rot2 = NullQ;
            rot3 = NullQ;

            return(false);
        }