internal static MyQuat InvertQuaternion(MyQuat q) { q.x = -q.x; q.y = -q.y; q.z = -q.z; return(q); }
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); }
//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; }
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); }
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); }
public static MyQuat GetSwing(MyQuat rot3) { //it's done in the pickstudanimvertical function return(rot3); }
//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); }