// Returns true if the IK is complete (either the end effector is at the target or it is not changing from its current position and there is no solution)
bool Skeleton::ProcessIK2D(MatrixBuffer& buffer, XMFLOAT2 rootPos, XMFLOAT2 destPos)
{
// The root position is determined by the pathing algorithm from Project 2 and set here.
// Since the object has already been moved in GraphicsSystem.cpp, the old, new and current root positions are all the same
m_OldJointPositions2D[0] = rootPos;
m_CurrentJointPositions2D[0] = rootPos;
m_TargetJointPositions2D[0] = rootPos;
XMFLOAT2 currentPos;
XMFLOAT2 newPos;
// This loop represents one iteration of Cyclic Coordinate Descent.
// The ProcessIK2D function must be called repeatedly for a solution.
for (int i = (int)m_TargetJointPositions2D.size() - 1; i >= 0; --i)
{
currentPos = CalculateTargetPosition2D();
// If the distance between the current and destination points < epsilon, exit
if (Mag(destPos - currentPos) < m_IKEpsilon)
{
return true;
}
Vector2 Vci = currentPos - m_TargetJointPositions2D[i];;
Vector2 Vdi = destPos - m_TargetJointPositions2D[i];
if (Mag(Vci) <= 0.f || Mag(Vdi) <= 0.f)
{
return false;
}
m_TargetJointRotations2D[i] = AngleInRadians(Vci, Vdi);
// Constrain rotation
if (i == 0) { m_TargetJointRotations2D[i] = PI / 2.f; }
else
{
// Closer to the root => less rotation allowed
float upperBound = PI * (float)i / (float)(m_TargetJointRotations2D.size() - 1);
float lowerBound = -upperBound;
Clamp(m_TargetJointRotations2D[i], lowerBound, upperBound);
}
// Check the new position
newPos = CalculateTargetPosition2D();
}
// If there is little difference between this iteration and the previous one, return
// true to indicate that the IK is complete
if (Mag(newPos - currentPos) < m_IKEpsilon)
{
return true;
}
return false;
}
double ON_Cone::AngleInDegrees() const
{
return 180.0*AngleInRadians()/ON_PI;
}
