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