Vector rotation problem - c++

Question

I'm working on a program with IK and have run into what I had at first thought was a trivial problem but have since had trouble solving it.
Background:
Everything is in 3d space.
I'm using 3d Vectors and Quaternions to represent transforms.
I have a limb which we will call V1.
I want to rotate it onto V2.
I was getting the angle between V1 and V2.
Then the axis for rotation by V1 cross V2.
Then making a Quaternion from the axis and angle.
I then take the limbs current Orientation and multiply it by the axis angle quaternion.
This I believe is my desired local space for the limb.
This limb is attached to a series of other links. To get the world space I traverse up to the root combining the parents local space with the child's local space until I reach the root.
This seems to work grand if the vector that I am rotating to is contained within the X and Y plane or if the body which the limb is attached to hasn't been modified. If anything has been modified, for example rotating the root node, then on the first iteration the vector will rotate very close to the desired vector. After that point though it will begin to spin all over the place and never reach the goal.
I've gone through all the math line by line and it appears to all be correct. I'm not sure if there is something that I do not know about or am simply over looking. Is my logical sound? Or am I unaware of something? Any help is greatly appreciated!
Quaternion::Quaternion(const Vector& axis, const float angle)
{
float sin_half_angle = sinf( angle / 2 );
v.set_x( axis.get_x() * sin_half_angle );
v.set_y( axis.get_y() * sin_half_angle );
v.set_z( axis.get_z() * sin_half_angle );
w = cosf( angle / 2 );
}
Quaternion Quaternion::operator* (const Quaternion& quat) const
{
Quaternion result;
Vector v1( this->v );
Vector v2( quat.v );
float s1 = this->w;
float s2 = quat.w;
result.w = s1 * s2 - v1.Dot(v2);
result.v = v2 * s1 + v1 * s2 + v1.Cross(v2);
result.Normalize();
return result;
}
Vector Quaternion::operator* (const Vector& vec) const
{
Quaternion quat_vec(vec.get_x(), vec.get_y(), vec.get_z(), 0.0f);
Quaternion rotation( *this );
Quaternion rotated_vec = rotation * ( quat_vec * rotation.Conjugate() );
return rotated_vec.v;
}
Quaternion Quaternion::Conjugate()
{
Quaternion result( *this );
result.v = result.v * -1.0f;
return result;
}
Transform Transform::operator*(const Transform tran)
{
return Transform( mOrient * transform.getOrient(), mTrans + ( mOrient * tran.getTrans());
}
Transform Joint::GetWorldSpace()
{
Transform world = local_space;
Joint* par = GetParent();
while ( par )
{
world = par->GetLocalSpace() * world;
par = par->GetParent();
}
return world;
}
void RotLimb()
{
Vector end_effector_worldspace_pos = end_effector->GetWorldSpace().get_Translation();
Vector parent_worldspace_pos = parent->GetWorldSpace().get_Translation();
Vector parent_To_end_effector = ( end_effector_worldspace_pos - parent_worldspace_pos ).Normalize();
Vector parent_To_goal = ( goal_pos - parent_worldspace_pos ).Normalize();
float dot = parent_To_end_effector.Dot( parent_To_goal );
Vector rot_axis(0.0f,0.0f,1.0f);
float angle = 0.0f;
if (1.0f - fabs(dot) > EPSILON)
{
//angle = parent_To_end_effector.Angle( parent_To_goal );
rot_axis = parent_To_end_effector.Cross( parent_To_goal ).Normalize();
parent->RotateJoint( rot_axis, acos(dot) );
}
}
void Joint::Rotate( const Vector& axis, const float rotation )
{
mLocalSpace = mlocalSpace * Quaternion( axis, rotation );
}

Answer 1

You are correct when you write in a comment that the axis should be computed in the local coordinate frame of the joint:
I'm wondering if this issue is occuring because I'm doing the calculations to get the axis and angle in the world space for the joint, but then applying it to the local space.
The rotation of the axis from the world frame to the joint frame will look something like this:
rot_axis = parent->GetWorldSpace().Inverse().get_Rotation() * rot_axis
There can be other issues to debug, but it's the only logical error I can see in the code that you have posted.