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