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);
}
