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