Search the Community

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.