# CCD IK algorithm going horribly wrong

I'm trying to implement the Cyclic Coordinate Descent algorithm described here:

http://www.cs.cmu.edu/~15464-s13/assignments/assignment2/jlander_gamedev_nov98.pdf

So first of all, I'm passing in the starting position bone which the CCD algorithm should stop at and the endPos which is the end-effector bone essentially. So in this instance, startPos could be taken from the Left_Clavicle bone and the endPos could be the Left_Hand. I also pass in the target position and a model object to be able to update the hierarchy.

Here is my attempt at the CCD:

void ComputeCCD(Model * model,glm::vec3 target, Bone * startPos, AnimNode * endPos)
{
glm::vec3 rootPos,curEnd,desiredEnd,targetVector,curVector,crossResult;
double cosAngle,turnAngle,turnDeg;
int tries;

Bone * currentBone = endPos->m_parent;
tries = 0;

do
{
rootPos = GetPosition(currentBone->m_globalTransform);
// POSITION OF THE END EFFECTOR
curEnd = GetPosition(endPos->m_globalTransform);
// DESIRED END EFFECTOR POSITION
desiredEnd = target;
float distSq = DistSquare(curEnd,desiredEnd);
// SEE IF I AM ALREADY CLOSE ENOUGH
if (distSq > IK_POS_THRESH)
{
// CREATE THE VECTOR TO THE CURRENT EFFECTOR POS
curVector = curEnd - rootPos;

// CREATE THE DESIRED EFFECTOR POSITION VECTOR
targetVector= target - rootPos;

cosAngle = glm::dot(targetVector,curVector);
turnAngle = glm::acos((float)cosAngle);
turnDeg = glm::degrees(turnAngle);
//Get axis of rotation
crossResult = glm::cross(targetVector,curVector);

//currentBone->m_localTransform *= glm::rotate(glm::mat4(1.0f),          (float)turnDeg,crossResult);

// IF THE DOT PRODUCT RETURNS 1.0, I DON'T NEED TO ROTATE AS IT IS 0 DEGREES
if (cosAngle < 0.99999)
{
if (crossResult.z > 0.0f) // IF THE Z ELEMENT IS POSITIVE, ROTATE CLOCKWISE
{

currentBone->m_localTransform = glm::rotate(currentBone->m_globalTransform,-(float)turnDeg,crossResult);
}
else if (crossResult.z < 0.0f) // ROTATE COUNTER CLOCKWISE
{
currentBone->m_localTransform =  glm::rotate(currentBone->m_globalTransform,(float)turnDeg,crossResult);
}

}
if (currentBone->m_name == startPos->m_name)
{
currentBone = endPos->m_parent;
// START OF THE CHAIN, RESTART
}
else
{
currentBone = currentBone->m_parent;
}
model->UpdateHierarchy(currentBone);
}

} while (tries++ < MAX_IK_TRIES && DistSquare(curEnd, desiredEnd) > IK_POS_THRESH);
}


So I have a tracker which I can move around and pass into this method. Whenever I try to run it I get the following error. The arm folds in on itself :(

Fixed it by replacing

 currentBone->m_localTransform = glm::rotate(currentBone->m_globalTransform,-  (float)turnDeg,crossResult);


with

 glm::mat4 rotation = glm::axisAngleMatrix(crossResult, (float)turnAngle);

currentBone->m_localTransform *= rotation;