IK Animation
Entry posted by Chris Paulson ·
3,825 views
To get nice locomotion and not have NPC walking with feet stuck in the terrain or hovering in the air you need to place the feet to match the terrain, coping with slopes etc.
Leadwerks has forward kinematics but not inverse (which is needed to do this). Because of this I have written (with help) an IK solver.
Thanks to Tyler for supplying the code as a starting point.
Here's a video of it working: -
Here's the code (I actually ended up doing 2 solvers)
#include "IKSimple.h"
#include "engine.h"
float IKSolver::positionAccuracy = 0.001f;
int IKSimple::maxIterations = 100;
float IKSimple::calcBonesLength()
{
boneLength.resize(bones.size()-1);
float totalLength = 0.0f;
for (int j = 0; j < (bones.size() - 1); j++)
{
Vector3 vector = bones[j + 1].localPosition() - bones[j].localPosition();
boneLength[j] = vector.Magnitude();
totalLength += boneLength[j];
}
this->positionAccuracy = totalLength * 0.001f;
return totalLength;
}
void IKSimple::Solve( TEntity entity, std::vector<Transform> pbones, Vector3 target)
{
/// Local Variables ///////////////////////////////////////////////////////////
Vector3 rootPos,curEnd,desiredEnd,targetVector,curVector,crossResult;
double cosAngle,turnAngle,turnDeg;
int link,tries;
Quaternion aquat;
///////////////////////////////////////////////////////////////////////////////
// START AT THE LAST LINK IN THE CHAIN
bones = pbones;
startBone = &pbones[0];
endBone = &pbones[pbones.size() - 1];
link = pbones.size() - 1;
tries = 0; // LOOP COUNTER SO I KNOW WHEN TO QUIT
do
{
// THE COORDS OF THE X,Y,Z POSITION OF THE ROOT OF THIS BONE IS IN THE MATRIX
// TRANSLATION PART WHICH IS IN THE 12,13,14 POSITION OF THE MATRIX
rootPos = pbones[link].localPosition();
// POSITION OF THE END EFFECTOR
curEnd = endBone->localPosition();
// DESIRED END EFFECTOR POSITION
desiredEnd = target;
// SEE IF I AM ALREADY CLOSE ENOUGH
if ( curEnd.DistanceSquared(desiredEnd) > positionAccuracy)
{
// CREATE THE VECTOR TO THE CURRENT EFFECTOR POS
curVector = curEnd - rootPos;
// CREATE THE DESIRED EFFECTOR POSITION VECTOR
targetVector = target - rootPos;
// NORMALIZE THE VECTORS (EXPENSIVE, REQUIRES A SQRT)
curVector.Normalize();
targetVector.Normalize();
// THE DOT PRODUCT GIVES ME THE COSINE OF THE DESIRED ANGLE
cosAngle = targetVector.Dot( curVector);
// IF THE DOT PRODUCT RETURNS 1.0, I DON'T NEED TO ROTATE AS IT IS 0 DEGREES
if (cosAngle < 0.99999)
{
// USE THE CROSS PRODUCT TO CHECK WHICH WAY TO ROTATE
crossResult = curVector.Cross(targetVector);
crossResult.Normalize();
turnAngle = acos((float)cosAngle); // GET THE ANGLE
turnDeg = rad2deg(turnAngle); // COVERT TO DEGREES
// DAMPING
turnDeg *= pbones[link].m_damper;
aquat = Quaternion::Quaternion(crossResult, turnDeg );
aquat = pbones[link].boneLocalRotation() * aquat;
pbones[link].setLocalRotation( aquat );
}
if (--link < 0) link = pbones.size() - 1; // START OF THE CHAIN, RESTART
}
// QUIT IF I AM CLOSE ENOUGH OR BEEN RUNNING LONG ENOUGH
} while (tries++ < maxIterations && curEnd.DistanceSquared( desiredEnd) > positionAccuracy);
}
void IKSimple::calcCurrentBones( )
{
rotateArray.resize(bones.size()-2);
angles.resize(bones.size()-2);
quaternionArray.resize(bones.size()-2);
// Work out where current bones are
for (int i = 0; i < (bones.size() - 2); i++)
{
rotateArray[i] = (bones[i + 1].localPosition() - bones[i].localPosition()).Cross(bones[i + 2].localPosition() - bones[i + 1].localPosition());
rotateArray[i] = (Vector3) ((bones[i].localRotation().Inverse()) * rotateArray[i]);
rotateArray[i].Normalize();
angles[i] = (bones[i + 1].localPosition() - bones[i].localPosition() ).Angle(bones[i + 1].localPosition() - bones[i + 2].localPosition() );
quaternionArray[i] = bones[i + 1].localRotation();
}
}
void IKSimple::SolveRelative( TEntity entity, std::vector<Transform> pbones, Vector3 target)
{
float doLow;
float doHigh;
bones = pbones;
startBone = &bones[0];
endBone = &bones[bones.size() - 1];
// Work out length of bones
float totalLength = calcBonesLength();
Vector3 curdis = endBone->localPosition() - startBone->localPosition();
float currentDistance = curdis.Magnitude();
Vector3 vtarget = target - startBone->localPosition();
float targetDistance = vtarget.Magnitude();
bool minFound = false;
bool moreToDo = false;
if (targetDistance > currentDistance)
{
minFound = true;
doHigh = 1.0f;
doLow = 0.0f;
}
else
{
moreToDo = true;
doHigh = 1.0f;
doLow = 0.0f;
}
int currentIter = 0;
while ((abs((float) (currentDistance - targetDistance)) > this->positionAccuracy) && (currentIter < this->maxIterations))
{
float newBend;
currentIter++;
if (!minFound)
newBend = doHigh;
else
newBend = (doLow + doHigh) / 2.0f;
for (int i = 0; i < (bones.size() - 2); i++)
{
float calcAngle;
if (!moreToDo)
{
calcAngle = math::Lerp(180.0f, angles[i], newBend);
}
else
{
calcAngle = (angles[i] * (1.0f - newBend)) + ((angles[i] - 30.0f) * newBend);
}
float angleDiff = angles[i] - calcAngle;
Quaternion newRot = Quaternion::Quaternion(rotateArray[i], angleDiff );
newRot = quaternionArray[i] * newRot;
bones[i + 1].setLocalRotation( newRot );
}
Vector3 totalLen = endBone->localPosition() - startBone->localPosition();
currentDistance = totalLen.Magnitude();
if (targetDistance > currentDistance)
minFound = true;
if (minFound) {
if (targetDistance > currentDistance)
doHigh = newBend;
else
doLow = newBend;
if (doHigh < 0.01f) break;
}
else {
doLow = doHigh;
doHigh++;
}
}
// Change master bone (if we was doing a leg this would be the hip)
float hipAngle = (endBone->localPosition() - startBone->localPosition()).Angle(target - startBone->localPosition());
Vector3 hipAxis = (endBone->localPosition() - startBone->localPosition()).Cross(target - startBone->localPosition());
AngleAxis hipAngleAxis = AngleAxis( hipAxis, hipAngle );
Quaternion hipRot = startBone->localRotation() * Quaternion::Quaternion( hipAngleAxis );
startBone->setLocalRotation( hipRot );
}
Here's the transform object which controls the bones. It was a little bit tricky to do because LE annoyingly does local rotation relative to the parent bone not relative to the model.
#ifndef TRANSFORM_H
#define TRANSFORM_H
/// @file locomotion\tranform.h
/// @brief This is the transform object which handles rotation/positioning of bones
/// for the IK solver
///
#include "SVector3.h"
#include "SQuaternion.h"
class Transform
{
public:
Transform() {};
Transform(TEntity parent, TEntity entity) : m_parent(parent)
{
m_entity = entity;
TVec3 v;
v = EntityScale(entity);
m_scale = Vector3(v.X,v.Y,v.Z);
v = Vec3(0,1,0);
v = TFormVector(v, entity, NULL);
m_up = Vector3(v.X,v.Y,v.Z);
v = Vec3(0,0,1);
v = TFormVector(v, entity, NULL);
m_forward = Vector3(v.X,v.Y,v.Z);
v = Vec3(1,0,0);
v = TFormVector(v, entity, NULL);
m_right = Vector3(v.X,v.Y,v.Z);
m_damper = 1;
m_minRotation = Vector3(-360,-360,-360);
m_maxRotation = Vector3(360,360,360);
}
inline Quaternion boneLocalRotation()
{
TVec3 v;
v = EntityRotation(m_entity,0);
Quaternion q = Quaternion( Vector3(v) );
return q;
}
inline Quaternion localRotation()
{
TVec3 v;
TEntity par = GetParent( m_entity );
EntityParent( m_entity, m_parent );
v = EntityRotation(m_entity,0);
EntityParent( m_entity, par );
Quaternion q = Quaternion( Vector3(v) );
return q;
}
inline Quaternion rotation()
{
TVec3 v;
v = EntityRotation(m_entity,1);
Quaternion q1 = Quaternion(Vector3(v));
return q1;
}
inline void alignToVector( Vector3 v, int axis = 3, int rate = 1, int roll = 0 )
{
AlignToVector( m_entity, v.vec3(), axis, rate, 0);
}
inline void setLocalRotation( Quaternion rot )
{
Vector3 vrot;
vrot = rot.eulerAngles();
vrot.X = __max( vrot.X, m_minRotation.X );
vrot.Y = __max( vrot.Y, m_minRotation.Y );
vrot.Z = __max( vrot.Z, m_minRotation.Z );
vrot.X = __min( vrot.X, m_maxRotation.X );
vrot.Y = __min( vrot.Y, m_maxRotation.Y );
vrot.Z = __min( vrot.Z, m_maxRotation.Z );
RotateEntity( m_entity, vrot.vec3(), 0);
}
inline void setRotation( Quaternion rot )
{
Vector3 vrot;
vrot = rot.eulerAngles();
RotateEntity( m_entity, vrot.vec3(), 1);
}
inline Vector3 position()
{
TVec3 v;
v = EntityPosition(m_entity,1);
v = TFormPoint(v ,NULL, m_parent);
return Vector3( v );
}
inline Vector3 localPosition()
{
TVec3 v;
v = EntityPosition(m_entity, 1);
v = TFormPoint(v, NULL, m_parent);
return Vector3( v );
}
TEntity m_entity;
Vector3 m_minRotation;
Vector3 m_maxRotation;
float m_damper;
private:
TEntity m_parent;
Vector3 m_scale;
Vector3 m_up;
Vector3 m_forward;
Vector3 m_right;
};
#endif
Just ask if you want me to supply the Vector3 and Quaternion math code if you need it.
5 Comments
Recommended Comments