/* Simulates the forward kinematics,
* given a solution. */
public PositionRotation ForwardKinematics(float[] Solution)
{
Vector3Class prevPoint = new Vector3Class(Joints[0].transform.position);
// Takes object initial rotation into account
QuaternionClass rotation = new QuaternionClass();
rotation.SetValues(transform.rotation);
for (int i = 1; i < Joints.Length; i++)
{
QuaternionClass angleAxis = new QuaternionClass();
angleAxis.convertFromAxisAngle(Joints[i - 1].Axis, Solution[i - 1]);
rotation *= angleAxis;
Vector3Class nextPoint = prevPoint + rotation.multiplyVec3(rotation, Joints[i].StartOffset);
if (DebugDraw)
{
Debug.DrawLine(prevPoint.GetValues(), nextPoint.GetValues(), Color.blue);
}
prevPoint = nextPoint;
}
// The end of the effector
return(new PositionRotation(prevPoint, rotation));
}
public void convertToAxisAngle(QuaternionClass q1)
{
float a;
Vector3 axis;
a = 2 * Mathf.Acos(q1.w);
axis.x = q1.x / Mathf.Sqrt(1 - Mathf.Pow(q1.w, 2));
axis.y = q1.y / Mathf.Sqrt(1 - Mathf.Pow(q1.w, 2));
axis.z = q1.z / Mathf.Sqrt(1 - Mathf.Pow(q1.w, 2));
}
public QuaternionClass inverse(QuaternionClass q1)
{
QuaternionClass q2 = new QuaternionClass();
//transpose
q2.w = q1.w;
q2.x = -q1.x;
q2.y = -q1.y;
q2.z = -q1.z;
return(multiply(q1, q2));
}
public QuaternionClass multiply(QuaternionClass q2, QuaternionClass q1)
{
QuaternionClass q3 = new QuaternionClass();
q3.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
q3.x = q1.w * q2.x + q1.x * q2.w - q1.y * q2.z + q1.z * q2.y;
q3.y = q1.w * q2.y + q1.x * q2.z + q1.y * q2.w - q1.z * q2.x;
q3.z = q1.w * q2.z - q1.x * q2.y + q1.y * q2.x - q1.z * q2.w;
return(q3);
}
public Vector3Class multiplyVec3(QuaternionClass quat, Vector3Class vec)
{
Vector3Class temp = new Vector3Class();
//rotation matrix
float xPart1 = 1 - 2 * Mathf.Pow(quat.y, 2) - 2 * Mathf.Pow(quat.z, 2);
float xPart2 = 2 * quat.x * quat.y - 2 * quat.z * quat.w;
float xPart3 = 2 * quat.x * quat.z + 2 * quat.y * quat.w;
float yPart1 = 2 * quat.x * quat.y + 2 * quat.z * quat.w;
float yPart2 = 1 - 2 * Mathf.Pow(quat.x, 2) - 2 * Mathf.Pow(quat.z, 2);
float yPart3 = 2 * quat.y * quat.z - 2 * quat.x * quat.w;
float zPart1 = 2 * quat.x * quat.z - 2 * quat.y * quat.w;
float zPart2 = 2 * quat.y * quat.z + 2 * quat.x * quat.w;
float zPart3 = 1 - 2 * Mathf.Pow(quat.x, 2) - 2 * Mathf.Pow(quat.y, 2);
//calculo vector
temp.x = xPart1 * vec.x + xPart2 * vec.y + xPart3 * vec.z;
temp.y = yPart1 * vec.x + yPart2 * vec.y + yPart3 * vec.z;
temp.z = zPart1 * vec.x + zPart2 * vec.y + zPart3 * vec.z;
return(temp);
}
public PositionRotation(Vector3Class position, QuaternionClass rotation)
{
this.position = position;
this.rotation = rotation;
}
