diff --git a/Engine/source/math/mQuat.cpp b/Engine/source/math/mQuat.cpp index d2b1a6ae6c..01a260258f 100644 --- a/Engine/source/math/mQuat.cpp +++ b/Engine/source/math/mQuat.cpp @@ -342,3 +342,40 @@ QuatF & QuatF::shortestArc( const VectorF &a, const VectorF &b ) return *this; } +QuatF& QuatF::computeRotationFromTo(const VectorF& from, const VectorF& to) +{ + VectorF f = from; + VectorF t = to; + + f.normalizeSafe(); + t.normalizeSafe(); + + if (f.isZero() || t.isZero()) + { + return identity(); + } + + F32 dot = mClampF(mDot(f, t), -1.0f, 1.0f); + + // Parallel = no rotation. + if (dot > 0.9999f) + { + return identity(); + } + + // Opposite = pick perpendicular. + if (dot < -0.9999f) + { + VectorF axis; + if (mFabs(f.x) < mFabs(f.z)) + axis.set(0, -f.z, f.y); + else + axis.set(-f.y, f.x, 0); + + axis.normalizeSafe(); + return set(axis, M_PI_F); // 180 degrees + } + + return shortestArc(f, t); +} + diff --git a/Engine/source/math/mQuat.h b/Engine/source/math/mQuat.h index 1af1a5a95f..ebc2578cc7 100644 --- a/Engine/source/math/mQuat.h +++ b/Engine/source/math/mQuat.h @@ -70,10 +70,12 @@ class QuatF QuatF& operator /=( F32 a ); QuatF operator-( const QuatF &c ) const; + QuatF operator*(const QuatF& rhs) const; QuatF operator*( F32 a ) const; QuatF& square(); QuatF& neg(); + QuatF& conjugate(); F32 dot( const QuatF &q ) const; MatrixF* setMatrix( MatrixF * mat ) const; @@ -91,6 +93,7 @@ class QuatF // Vectors passed in must be normalized QuatF& shortestArc( const VectorF &normalizedA, const VectorF &normalizedB ); + QuatF& computeRotationFromTo(const VectorF& from, const VectorF& to); }; // a couple simple utility methods @@ -208,6 +211,18 @@ inline QuatF QuatF::operator -( const QuatF &c ) const w - c.w ); } +inline QuatF QuatF::operator*(const QuatF& rhs) const +{ + QuatF out; + + out.w = w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z; + out.x = w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y; + out.y = w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x; + out.z = w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w; + + return out; +} + inline QuatF QuatF::operator *( F32 a ) const { return QuatF( x * a, @@ -225,6 +240,14 @@ inline QuatF& QuatF::neg() return *this; } +inline QuatF& QuatF::conjugate() +{ + x = -x; + y = -y; + z = -z; + return *this; +} + inline F32 QuatF::dot( const QuatF &q ) const { return mClampF(w*q.w + x*q.x + y*q.y + z*q.z, -1.0f, 1.0f); diff --git a/Engine/source/ts/tsAnimate.cpp b/Engine/source/ts/tsAnimate.cpp index ef88d1eb8a..2a8533d75f 100644 --- a/Engine/source/ts/tsAnimate.cpp +++ b/Engine/source/ts/tsAnimate.cpp @@ -866,8 +866,21 @@ void TSShapeInstance::animate(S32 dl) // animate nodes? if (dirtyFlags & TransformDirty) + { animateNodes(ss); + //--------------------------------------- + // TODO: Implement different ik methods + // add limits to ik chain nodes + // cache bone lengths. + //--------------------------------------- + for (U32 i = 0; i < mShape->ikChains.size(); i++) + { + if (mShape->ikChains[i].enabled) + solveCCD(mShape->ikChains[i]); + } + } + // animate objects? if (dirtyFlags & VisDirty) animateVisibility(ss); diff --git a/Engine/source/ts/tsIKSolver.cpp b/Engine/source/ts/tsIKSolver.cpp new file mode 100644 index 0000000000..7ce034ad8c --- /dev/null +++ b/Engine/source/ts/tsIKSolver.cpp @@ -0,0 +1,292 @@ +#include "ts/tsShapeInstance.h" + +/// All ik solving happens to the global transform then is converted to localspace +/// mNodeTransforms is the global transform vector +/// smNodeLocalTransform is the local transform vector +/// we do our solving after animateNodes so animations can change bone lengths +/// but our fabrik solver does not! +/// The book C++ Game Animation Programming 2nd Edition was used as reference. + +Vector TSShapeInstance::smFabrikBoneLengths(__FILE__, __LINE__); +Vector TSShapeInstance::smFabrikPositions(__FILE__, __LINE__); +Vector TSShapeInstance::smIKChainNodes(__FILE__, __LINE__); + +//------------------------------------------------------------------- +// FABRIK FUNCTIONS +//------------------------------------------------------------------- + +void TSShapeInstance::updateChildWorldTransforms(S32 node) +{ + for (S32 child = mShape->nodes[node].firstChild; child != -1; child = mShape->nodes[child].nextSibling) + { + // world[child] = world[parent] * local[child] + mNodeTransforms[child].mul(mNodeTransforms[node], smNodeLocalTransforms[child]); + + // recurse + updateChildWorldTransforms(child); + } +} + +bool TSShapeInstance::calculateFabrikBoneLengths() +{ + const S32 count = smIKChainNodes.size(); + + // not enough for fabrik. + if (count < 2) + return false; + + smFabrikBoneLengths.clear(); + smFabrikBoneLengths.setSize(count - 1); + + for (S32 i = 0; i < count - 1; i++) + { + const Point3F p0 = mNodeTransforms[smIKChainNodes[i]].getPosition(); + const Point3F p1 = mNodeTransforms[smIKChainNodes[i + 1]].getPosition(); + smFabrikBoneLengths[i] = (p1 - p0).magnitudeSafe(); + } + return true; +} + +void TSShapeInstance::applyFabrik(TSShape::IKChain& chain) +{ + const U32 count = smFabrikPositions.size(); + if (count < 2) + return; + + for (S32 i = count - 1; i > 0; i--) + { + S32 nodeIdx = smIKChainNodes[i]; + S32 nextNodeIdx = smIKChainNodes[i-1]; + + // Current joint global rotation & position + QuatF curWorldRot; + curWorldRot.set(mNodeTransforms[nodeIdx]); + const Point3F curPos = mNodeTransforms[nodeIdx].getPosition(); + const Point3F nextPos = mNodeTransforms[nextNodeIdx].getPosition(); + + // Direction vectors + VectorF toNext = nextPos - curPos; + toNext.normalizeSafe(); + + VectorF toDesired = smFabrikPositions[i - 1] - smFabrikPositions[i]; + toDesired.normalizeSafe(); + + // Rotation direction -> target direction + QuatF rotToTarget; + rotToTarget.computeRotationFromTo(toDesired, toNext); + + QuatF parentWorldRot; + S32 parentIdx = mShape->nodes[nodeIdx].parentIndex; + if (parentIdx >= 0) + parentWorldRot.set(mNodeTransforms[parentIdx]); + else + parentWorldRot.identity(); + + QuatF curRotLocal = smNodeCurrentRotations[nodeIdx]; + QuatF newRotLocal = parentWorldRot * rotToTarget * parentWorldRot.conjugate(); + + QuatF blendedRot; + + TSTransform::interpolate(curRotLocal, curRotLocal * newRotLocal, chain.weight, &blendedRot); + + // Update local rotation & matrix + smNodeCurrentRotations[nodeIdx] = blendedRot; + TSTransform::setMatrix(blendedRot, smNodeCurrentTranslations[nodeIdx], &smNodeLocalTransforms[nodeIdx]); + + // Update world transform for this node + if (parentIdx >= 0) + mNodeTransforms[nodeIdx].mul(mNodeTransforms[parentIdx], smNodeLocalTransforms[nodeIdx]); + else + mNodeTransforms[nodeIdx] = smNodeLocalTransforms[nodeIdx]; + + // Propagate world transform to children + updateChildWorldTransforms(nodeIdx); + } +} + +void TSShapeInstance::solveFabrikForward(const Point3F& target) +{ + const U32 count = smFabrikPositions.size(); + if (count < 2) + return; + + smFabrikPositions[0] = target; + + for (S32 i = 1; i < count; i++) + { + VectorF direction = (smFabrikPositions[i] - smFabrikPositions[i - 1]); + direction.normalizeSafe(); + + Point3F offset = direction * smFabrikBoneLengths[i - 1]; + + smFabrikPositions[i] = smFabrikPositions[i - 1] + offset; + } +} + +void TSShapeInstance::solveFabrikBackward(const Point3F& root) +{ + const U32 count = smFabrikPositions.size(); + if (count < 2) + return; + + smFabrikPositions[count - 1] = root; + + for (S32 i = count-2; i > 0; i--) + { + VectorF direction = (smFabrikPositions[i] - smFabrikPositions[i + 1]); + direction.normalizeSafe(); + + Point3F offset = direction * smFabrikBoneLengths[i]; + + smFabrikPositions[i] = smFabrikPositions[i + 1] + offset; + } + +} + +//------------------------------------------------------------------- +// FABRIK FUNCTIONS END +//------------------------------------------------------------------- + +//------------------------------------------------------------------- +// MAIN SOLVER ENTRY FUNCTIONS +//------------------------------------------------------------------- + +bool TSShapeInstance::solveCCD(TSShape::IKChain& chain) +{ + PROFILE_SCOPE(TSShapeInstance_solveCCD); + + if (!chain.enabled) + return false; + + // safety. + smIKChainNodes.clear(); + smIKChainNodes = chain.nodes; + + const U32 count = smIKChainNodes.size(); + if (count == 0) return false; + + const S32 endNode = smIKChainNodes.first(); + const F32 threshold = chain.threshold; + const U32 maxIterations = chain.maxIterations; + const Point3F targetPos = mNodeTransforms[chain.targetIndex].getPosition(); + + for (U32 iter = 0; iter < maxIterations; iter++) + { + Point3F endPos = mNodeTransforms[endNode].getPosition(); + + // Early exit if the end effector is close enough + if ((targetPos - endPos).magnitudeSafe() < threshold) + return true; + + // Iterate joints from the one before the end effector -> root + // (exclude the end effector itself) + for (S32 i = 1; i < count; i++) + { + S32 nodeIdx = smIKChainNodes[i]; + + // Current joint global rotation & position + QuatF curWorldRot; + curWorldRot.set(mNodeTransforms[nodeIdx]); + const Point3F curPos = mNodeTransforms[nodeIdx].getPosition(); + + // Direction vectors + VectorF toEnd = endPos - curPos; + VectorF toTarget = targetPos - curPos; + + toEnd.normalizeSafe(); + toTarget.normalizeSafe(); + + // Rotation direction -> target direction + QuatF rotToTarget; + rotToTarget.computeRotationFromTo(toTarget, toEnd); + + QuatF parentWorldRot; + S32 parentIdx = mShape->nodes[nodeIdx].parentIndex; + if (parentIdx >= 0) + parentWorldRot.set(mNodeTransforms[parentIdx]); + else + parentWorldRot.identity(); + + QuatF curRotLocal = smNodeCurrentRotations[nodeIdx]; + QuatF newRotLocal = parentWorldRot * rotToTarget * parentWorldRot.conjugate(); + + QuatF blendedRot; + + TSTransform::interpolate(curRotLocal, curRotLocal * newRotLocal, chain.weight, &blendedRot); + + // Update local rotation & matrix + smNodeCurrentRotations[nodeIdx] = blendedRot; + TSTransform::setMatrix(blendedRot, smNodeCurrentTranslations[nodeIdx], &smNodeLocalTransforms[nodeIdx]); + + // Update world transform for this node + if (parentIdx >= 0) + mNodeTransforms[nodeIdx].mul(mNodeTransforms[parentIdx], smNodeLocalTransforms[nodeIdx]); + else + mNodeTransforms[nodeIdx] = smNodeLocalTransforms[nodeIdx]; + + // Propagate world transform to children + updateChildWorldTransforms(nodeIdx); + + // Check updated end effector + endPos = mNodeTransforms[endNode].getPosition(); + if ((targetPos - endPos).magnitudeSafe() < threshold) + return true; + } + } + return false; +} + +bool TSShapeInstance::solveFrabrik(TSShape::IKChain& chain) +{ + PROFILE_SCOPE(TSShapeInstance_solveFabrik); + if (!chain.enabled) + return false; + + // safety. + smIKChainNodes.clear(); + + smIKChainNodes = chain.nodes; + const U32 count = smIKChainNodes.size(); + + // not enough for fabrik. + if (count < 2) + return false; + + // Resize working position buffer + smFabrikPositions.setSize(count); + + // Fill initial global positions + for (U32 i = 0; i < count; i++) + smFabrikPositions[i] = mNodeTransforms[smIKChainNodes[i]].getPosition(); + + if (!calculateFabrikBoneLengths()) + return false; + + const S32 endNode = smIKChainNodes.first(); + const Point3F rootPos = smFabrikPositions[count-1]; + const Point3F targetPos = mNodeTransforms[chain.targetIndex].getPosition(); + const F32 threshold = chain.threshold; + const U32 maxIterations = chain.maxIterations; + + for (U32 i = 0; i < maxIterations; i++) + { + Point3F endPos = smFabrikPositions[0]; + // Early exit if the end effector is close enough + if ((targetPos - endPos).magnitudeSafe() < threshold) + { + applyFabrik(chain); + return true; + } + + solveFabrikForward(targetPos); + solveFabrikBackward(rootPos); + } + + applyFabrik(chain); + + Point3F endPos = mNodeTransforms[endNode].getPosition(); + if ((targetPos - endPos).magnitudeSafe() < threshold) + return true; + + return false; +} diff --git a/Engine/source/ts/tsShape.cpp b/Engine/source/ts/tsShape.cpp index 19b873a336..409d11b970 100644 --- a/Engine/source/ts/tsShape.cpp +++ b/Engine/source/ts/tsShape.cpp @@ -101,6 +101,7 @@ TSShape::TSShape() VECTOR_SET_ASSOCIATION(billboardDetails); VECTOR_SET_ASSOCIATION(detailCollisionAccelerators); VECTOR_SET_ASSOCIATION(names); + VECTOR_SET_ASSOCIATION(ikChains); VECTOR_SET_ASSOCIATION( nodes ); VECTOR_SET_ASSOCIATION( objects ); @@ -203,6 +204,17 @@ const String& TSShape::getSequenceName( S32 seqIndex ) const return names[nameIdx]; } +const String& TSShape::getIKChainName(S32 ikIndex) const +{ + AssertFatal(ikIndex >= 0 && ikIndex < ikChains.size(), "TSShape::getIKChainName index beyond range"); + + S32 nameIdx = ikChains[ikIndex].nameIndex; + if (nameIdx < 0) + return String::EmptyString; + + return names[nameIdx]; +} + S32 TSShape::findName(const String &name) const { for (S32 i=0; i + /// IKChain struct for building an ik chain. + /// + /// A structure for holding our ik required data. + /// The index of the root node. + /// The index of the end node. + /// Vector of nodes making up the chain, end->root order. + /// The weight of the ik. + /// The minimum distance to consider as solved. + /// The maximum number of iterations to try to solve. + /// The index of the target node. + /// True if you want this chain to have an effect. + struct IKChain { + S32 nameIndex; + S32 rootNode; + S32 endNode; + Vector nodes; + F32 weight; + F32 threshold; + S32 maxIterations; + S32 targetIndex; + bool enabled; + }; + /// @name Shape Vector Data /// @{ @@ -346,6 +370,7 @@ class TSShape Vector billboardDetails; Vector detailCollisionAccelerators; Vector names; + Vector ikChains; /// @} @@ -529,6 +554,9 @@ class TSShape /// Returns name string for sequence at the passed index. const String& getSequenceName( S32 seqIndex ) const; + // returns name string for the ikchain at the passed index. + const String& getIKChainName(S32 ikIndex) const; + S32 getTargetCount() const; const String& getTargetName( S32 mapToNameIndex ) const; @@ -545,6 +573,9 @@ class TSShape S32 findSequence(S32 nameIndex) const; S32 findSequence(const String &name) const { return findSequence(findName(name)); } + S32 findIKChain(S32 nameIndex) const; + S32 findIKChain(const String& name) const { return findIKChain(findName(name)); } + S32 getSubShapeForNode(S32 nodeIndex); S32 getSubShapeForObject(S32 objIndex); void getSubShapeDetails(S32 subShapeIndex, Vector& validDetails); @@ -685,6 +716,21 @@ class TSShape bool addSequence(const Torque::Path& path, const String& assetId, const String& fromSeq, const String& name, S32 startFrame, S32 endFrame, bool padRotKeys, bool padTransKeys); bool removeSequence(const String& name); + /// + /// Convenience function to make sure nodes are related to each other. + /// + /// Ancestor node index to test. + /// Descendent node index to test. + /// true if the nodes are related, otherwise false. + bool isAncestorOf(S32 ancestor, S32 descendant) const; + bool addIKChain(const String& name, const String& nodeA, const String& nodeB); + bool removeIKChain(const String& name); + bool setIKChainEnabled(const String& name, bool isEnabled); + bool setIKChainWeight(const String& name, F32 weight); + bool setIKChainThreshold(const String& name, F32 threshold); + bool setIKChainMaxIterations(const String& name, S32 maxIterations); + bool setIKChainTarget(const String& name, const String& targetNode); + bool addTrigger(const String& seqName, S32 keyframe, S32 state); bool removeTrigger(const String& seqName, S32 keyframe, S32 state); diff --git a/Engine/source/ts/tsShapeConstruct.cpp b/Engine/source/ts/tsShapeConstruct.cpp index 3e11f1f599..524963001c 100644 --- a/Engine/source/ts/tsShapeConstruct.cpp +++ b/Engine/source/ts/tsShapeConstruct.cpp @@ -679,6 +679,19 @@ TSShape::Sequence* var = &(mShape->sequences[var##Index]); \ TORQUE_UNUSED(var##Index); \ TORQUE_UNUSED(var); +// Do an IKChain lookup +#define GET_IKCHAIN(func, var, name, ret) \ +S32 var##Index = mShape->findIKChain(name); \ +if (var##Index < 0) \ +{ \ +Con::errorf( "TSShapeConstructor::" #func ": Could not find " \ +"ikchain named '%s'", name); \ +return ret; \ +} \ +TSShape::IKChain* var = &(mShape->ikChains[var##Index]); \ +TORQUE_UNUSED(var##Index); \ +TORQUE_UNUSED(var); + //----------------------------------------------------------------------------- // DUMP @@ -2298,6 +2311,164 @@ DefineEngineFunction(findShapeConstructorByFilename, S32, (const char* filename) return 0; } +//------------------------------------------------------------------------------ +// IK CHAIN FUNCTIONS +//------------------------------------------------------------------------------ +DefineTSShapeConstructorMethod(getIKChainCount, S32, (), , (), 0, + "Get the total number of ikChains in the shape.\n" + "@return the number of ikChains in the shape\n\n") +{ + return mShape->ikChains.size(); +}} + +DefineTSShapeConstructorMethod(getIKChainIndex, S32, (const char* name), , + (name), -1, + "Find the index of the ikchain with the given name.\n" + "@param name name of the ikchain to lookup\n" + "@return index of the ikchain with matching name, or -1 if not found\n\n" + "@tsexample\n" + "// Check if a given sequence exists in the shape\n" + "if ( %this.getIKChainIndex( \"walk\" ) == -1 )\n" + " echo( \"Could not find 'foot_ik' ikchain\" );\n" + "@endtsexample\n") +{ + return mShape->findIKChain(name); +}} + +DefineTSShapeConstructorMethod(getIKChainName, const char*, (S32 index), , + (index), "", + "Get the name of the indexed ikchain.\n" + "@param index index of the ikchain to query (valid range is 0 - getIKChainCount()-1)\n" + "@return the name of the ikchain\n\n" + "@tsexample\n" + "// print the name of all ikchain in the shape\n" + "%count = %this.getIKChainCount();\n" + "for ( %i = 0; %i < %count; %i++ )\n" + " echo( %i SPC %this.getIKChainName( %i ) );\n" + "@endtsexample\n") +{ + CHECK_INDEX_IN_RANGE(getIKChainName, index, mShape->ikChains.size(), ""); + + return mShape->getName(mShape->ikChains[index].nameIndex); +}} + +DefineTSShapeConstructorMethod(addIKChain, bool, (const char* name, const char* nodeAName, const char* nodeBName), , + (name, nodeAName, nodeBName), false, + "Add a new ik chain.\n" + "@param name name of the sequence to modify\n" + "@param nodeAName root node for the chain\n" + "@param nodeBName end node for the chain\n" + "@return true if successful, false otherwise\n\n" + "@tsexample\n" + "%this.addIKChain( \"RightLegChain\", \"right_hip\", \"right_foot\" );\n" + "%this.addIKChain( \"LeftLegChain\", \"left_hip\", \"left_foot\" );\n" + "@endtsexample\n") +{ + if (!mShape->addIKChain(name, nodeAName, nodeBName)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +DefineTSShapeConstructorMethod(setIKChainWeight, bool, (const char* name, F32 weight), , + (name, weight), false, + "Set the ikchain weight.\n" + "@param name name of the ikchain to modify\n" + "@param weight new weight value\n" + "@return true if successful, false otherwise\n\n") +{ + GET_IKCHAIN(setIKChainWeight, ikchain, name, false); + + if (!mShape->setIKChainWeight(name, weight)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +DefineTSShapeConstructorMethod(setIKChainEnabled, bool, (const char* name, bool isEnabled), , + (name, isEnabled), false, + "Set the ikchain enabled status.\n" + "@param name name of the ikchain to modify\n" + "@param isEnabled new enabled value\n" + "@return true if successful, false otherwise\n\n") +{ + GET_IKCHAIN(setIKChainEnabled, ikchain, name, false); + + if (!mShape->setIKChainEnabled(name, isEnabled)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +DefineTSShapeConstructorMethod(setIKChainThreshold, bool, (const char* name, F32 threshold), , + (name, threshold), false, + "Set the ikchain threshold.\n" + "@param name name of the ikchain to modify\n" + "@param threshold new threshold value\n" + "@return true if successful, false otherwise\n\n") +{ + GET_IKCHAIN(setIKChainThreshold, ikchain, name, false); + + if (!mShape->setIKChainThreshold(name, threshold)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +DefineTSShapeConstructorMethod(setIKChainMaxIterations, bool, (const char* name, S32 iterations), , + (name, iterations), false, + "Set the ikchain threshold.\n" + "@param name name of the ikchain to modify\n" + "@param iterations new iterations value\n" + "@return true if successful, false otherwise\n\n") +{ + GET_IKCHAIN(setIKChainMaxIterations, ikchain, name, false); + + if (!mShape->setIKChainMaxIterations(name, iterations)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +DefineTSShapeConstructorMethod(setIKChainTarget, bool, (const char* name, const char* targetName), , + (name, targetName), false, + "Set the ikchain threshold.\n" + "@param name name of the ikchain to modify\n" + "@param targetName new target node value\n" + "@return true if successful, false otherwise\n\n") +{ + GET_IKCHAIN(setIKChainTarget, ikchain, name, false); + + if (!mShape->setIKChainTarget(name, targetName)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + + +DefineTSShapeConstructorMethod(removeIKChain, bool, (const char* name), , + (name), false, + "Remove an ikchain from the shape.\n" + "@param name name of the ikchain to remove.\n" + "@return true if successful, false otherwise.\n\n") +{ + if (!mShape->removeIKChain(name)) + return false; + + ADD_TO_CHANGE_SET(); + return true; +}} + +//------------------------------------------------------------------------------ +// IK CHAIN FUNCTIONS END +//------------------------------------------------------------------------------ + //----------------------------------------------------------------------------- // Change-Set manipulation TSShapeConstructor::ChangeSet::eCommandType TSShapeConstructor::ChangeSet::getCmdType(const char* name) @@ -2337,6 +2508,14 @@ else RETURN_IF_MATCH(SetSequenceBlend); else RETURN_IF_MATCH(SetSequencePriority); else RETURN_IF_MATCH(SetSequenceGroundSpeed); +else RETURN_IF_MATCH(AddIKChain); +else RETURN_IF_MATCH(RemoveIKChain); +else RETURN_IF_MATCH(SetIKChainWeight); +else RETURN_IF_MATCH(SetIKChainEnabled); +else RETURN_IF_MATCH(SetIKChainThreshold); +else RETURN_IF_MATCH(SetIKChainMaxIterations); +else RETURN_IF_MATCH(SetIKChainTarget); + else RETURN_IF_MATCH(AddTrigger); else RETURN_IF_MATCH(RemoveTrigger); @@ -2461,6 +2640,14 @@ void TSShapeConstructor::ChangeSet::add( TSShapeConstructor::ChangeSet::Command& case CmdRenameSequence: addCommand = addCmd_renameSequence(cmd); break; case CmdRemoveSequence: addCommand = addCmd_removeSequence(cmd); break; + case CmdAddIKChain: addCommand = addCmd_addIKChain(cmd); break; + case CmdRemoveIKChain: addCommand = addCmd_removeIKChain(cmd); break; + case CmdSetIKChainWeight: addCommand = addCmd_setIKChainWeight(cmd); break; + case CmdSetIKChainEnabled: addCommand = addCmd_setIKChainEnabled(cmd); break; + case CmdSetIKChainThreshold: addCommand = addCmd_setIKChainThreshold(cmd); break; + case CmdSetIKChainMaxIterations: addCommand = addCmd_setIKChainMaxIterations(cmd); break; + case CmdSetIKChainTarget: addCommand = addCmd_setIKChainTarget(cmd); break; + case CmdAddTrigger: addCommand = addCmd_addTrigger(cmd); break; case CmdRemoveTrigger: addCommand = addCmd_removeTrigger(cmd); break; @@ -2903,6 +3090,176 @@ bool TSShapeConstructor::ChangeSet::addCmd_removeSequence(const TSShapeConstruct return true; } +bool TSShapeConstructor::ChangeSet::addCmd_addIKChain(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdAddIKChain: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + return false; // command already exists + } + break; + case CmdRemoveIKChain: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + mCommands.erase(index); // Remove previous removeIKChain command + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_setIKChainWeight(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdSetIKChainWeight: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + cmd.argv[1] = newCmd.argv[1]; // Collapse successive set weight commands + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_setIKChainEnabled(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdSetIKChainEnabled: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + cmd.argv[1] = newCmd.argv[1]; // Collapse successive set enabled commands + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_setIKChainThreshold(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdSetIKChainThreshold: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + cmd.argv[1] = newCmd.argv[1]; // Collapse successive set threshold commands + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_setIKChainMaxIterations(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdSetIKChainMaxIterations: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + cmd.argv[1] = newCmd.argv[1]; // Collapse successive set max iterations commands + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_setIKChainTarget(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdSetIKChainTarget: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + cmd.argv[1] = newCmd.argv[1]; // Collapse successive set target commands + return false; + } + break; + default: + break; + } + } + + return true; +} + +bool TSShapeConstructor::ChangeSet::addCmd_removeIKChain(const Command& newCmd) +{ + for (S32 index = mCommands.size() - 1; index >= 0; index--) + { + Command& cmd = mCommands[index]; + switch (cmd.type) + { + case CmdAddIKChain: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + { + mCommands.erase(index); // Remove previous addIKChain command + return false; + } + break; + + case CmdSetIKChainWeight: + case CmdSetIKChainEnabled: + case CmdSetIKChainThreshold: + case CmdSetIKChainMaxIterations: + case CmdSetIKChainTarget: + if (namesEqual(cmd.argv[0], newCmd.argv[0])) + mCommands.erase(index); // Remove any commands that reference the removed sequence + break; + + default: + break; + } + } + + return true; +} + bool TSShapeConstructor::ChangeSet::addCmd_addTrigger(const TSShapeConstructor::ChangeSet::Command& newCmd) { // Remove a matching removeTrigger command, but stop if the sequence is used as diff --git a/Engine/source/ts/tsShapeConstruct.h b/Engine/source/ts/tsShapeConstruct.h index f4f8dfbffb..771a84943d 100644 --- a/Engine/source/ts/tsShapeConstruct.h +++ b/Engine/source/ts/tsShapeConstruct.h @@ -90,6 +90,14 @@ class TSShapeConstructor : public SimObject CmdSetSequencePriority, CmdSetSequenceGroundSpeed, + CmdAddIKChain, + CmdRemoveIKChain, + CmdSetIKChainWeight, + CmdSetIKChainEnabled, + CmdSetIKChainThreshold, + CmdSetIKChainMaxIterations, + CmdSetIKChainTarget, + CmdAddTrigger, CmdRemoveTrigger, @@ -156,6 +164,14 @@ class TSShapeConstructor : public SimObject bool addCmd_renameSequence(const Command& newCmd); bool addCmd_removeSequence(const Command& newCmd); + bool addCmd_addIKChain(const Command& newCmd); + bool addCmd_setIKChainWeight(const Command& newCmd); + bool addCmd_setIKChainEnabled(const Command& newCmd); + bool addCmd_setIKChainThreshold(const Command& newCmd); + bool addCmd_setIKChainMaxIterations(const Command& newCmd); + bool addCmd_setIKChainTarget(const Command& newCmd); + bool addCmd_removeIKChain(const Command& newCmd); + bool addCmd_addTrigger(const Command& newCmd); bool addCmd_removeTrigger(const Command& newCmd); @@ -340,6 +356,20 @@ class TSShapeConstructor : public SimObject bool removeSequence(const char* name); ///@} + /// @name IKChains + ///@{ + S32 getIKChainCount(); + S32 getIKChainIndex(const char* name); + const char* getIKChainName(S32 index); + bool addIKChain(const char* source, const char* nodeAName, const char* nodeBName); + bool setIKChainWeight(const char* name, F32 weight); + bool setIKChainEnabled(const char* name, bool isEnabled); + bool setIKChainThreshold(const char* name, F32 threshold); + bool setIKChainMaxIterations(const char* name, S32 maxIterations); + bool setIKChainTarget(const char* name, const char* targetNode); + bool removeIKChain(const char* name); + ///@} + /// @name Triggers ///@{ S32 getTriggerCount(const char* name); diff --git a/Engine/source/ts/tsShapeEdit.cpp b/Engine/source/ts/tsShapeEdit.cpp index db4c615493..8a8c4d70d4 100644 --- a/Engine/source/ts/tsShapeEdit.cpp +++ b/Engine/source/ts/tsShapeEdit.cpp @@ -31,7 +31,7 @@ #include "core/stream/fileStream.h" #include "core/volume.h" #include "assets/assetManager.h" - +#include "ts/tsShapeConstruct.h" //----------------------------------------------------------------------------- @@ -1880,6 +1880,219 @@ bool TSShape::removeSequence(const String& name) return true; } +//----------------------------------------------------------------------------- + +bool TSShape::isAncestorOf(S32 ancestor, S32 descendant) const +{ + S32 n = descendant; + while (n != -1) + { + if (n == ancestor) + return true; + n = nodes[n].parentIndex; + } + return false; +} + +bool TSShape::addIKChain(const String& name, const String& nodeAName, const String& nodeBName) +{ + // Check that there is not already an ikchain with this name + if (findIKChain(name) >= 0) + { + Con::errorf("TSShape::addIKChain: %s already exists!", name.c_str()); + return false; + } + + S32 nodeA = findNode(nodeAName); + S32 nodeB = findNode(nodeBName); + + if (nodeA < 0) + { + Con::errorf("TSShape::addIKChain: bone '%s' not found.", nodeAName.c_str()); + return false; + } + + if (nodeB < 0) + { + Con::errorf("TSShape::addIKChain: bone '%s' not found.", nodeBName.c_str()); + return false; + } + + S32 rootNode = -1; + S32 endNode = -1; + + if (isAncestorOf(nodeA, nodeB)) + { + rootNode = nodeA; + endNode = nodeB; + } + else if (isAncestorOf(nodeB, nodeA)) + { + rootNode = nodeB; + endNode = nodeA; + } + else + { + Con::errorf("TSShape::addIKChain: '%s' and '%s' are not related in the skeleton hierarchy.", + nodeAName.c_str(), nodeBName.c_str()); + return false; + } + + Vector chainNodes; + S32 n = endNode; + // loop through parents to get the root. + while (n != -1) + { + chainNodes.push_back(n); + if (n == rootNode) + break; + n = nodes[n].parentIndex; + } + + String targetNodeName = name + "_target"; + S32 targetNode = findNode(targetNodeName); + if (targetNode < 0) + { + Con::warnf("TSShape::addIKChain: default target node '%s' not found, adding one.", targetNodeName.c_str()); + TransformF txfm = TransformF::Identity; + Point3F pos(txfm.getPosition()); + QuatF rot(txfm.getOrientation()); + addNode(targetNodeName, "", pos, rot); + + targetNode = findNode(targetNodeName); + } + + //------------------------------------ + // Create IK Chain. + //------------------------------------ + IKChain chain; + chain.nameIndex = addName(name); + chain.rootNode = rootNode; + chain.endNode = endNode; + chain.nodes = chainNodes; + chain.weight = 1.0f; + chain.threshold = 0.1f; + chain.maxIterations = 10; + chain.targetIndex = targetNode; + chain.enabled = true; + + ikChains.push_back(chain); + + return true; +} + +bool TSShape::removeIKChain(const String& name) +{ + // Find the default target node, only remove this node as the ikchain added it. + String targetNodeName = name + "_target"; + S32 targetNode = findNode(targetNodeName); + if (targetNode >= 0) + { + removeNode(targetNodeName); + } + + // Find the ikchain to be removed + S32 ikChainIndex = findIKChain(name); + if (ikChainIndex < 0) + { + Con::errorf("TSShape::removeIKChain: Could not find ikchain '%s'", name.c_str()); + return false; + } + + ikChains.erase(ikChainIndex); + + // Remove the ikchain name if it is no longer in use + removeName(name); + + return true; +} + +bool TSShape::setIKChainEnabled(const String& name, bool isEnabled) +{ + S32 ikIndex = findIKChain(name); + if (ikIndex < 0) + { + Con::errorf("TSShape::setIKChainEnabled: Could not find ikchain named '%s'", name.c_str()); + return false; + } + + TSShape::IKChain& ikChain = ikChains[ikIndex]; + + ikChain.enabled = isEnabled; + + return true; +} + +bool TSShape::setIKChainWeight(const String& name, F32 weight) +{ + S32 ikIndex = findIKChain(name); + if (ikIndex < 0) + { + Con::errorf("TSShape::setIKChainEnabled: Could not find ikchain named '%s'", name.c_str()); + return false; + } + + TSShape::IKChain& ikChain = ikChains[ikIndex]; + + ikChain.weight = weight; + + return true; +} + +bool TSShape::setIKChainThreshold(const String& name, F32 threshold) +{ + S32 ikIndex = findIKChain(name); + if (ikIndex < 0) + { + Con::errorf("TSShape::setIKChainThreshold: Could not find ikchain named '%s'", name.c_str()); + return false; + } + + TSShape::IKChain& ikChain = ikChains[ikIndex]; + + ikChain.threshold = threshold; + + return true; +} + +bool TSShape::setIKChainMaxIterations(const String& name, S32 maxIterations) +{ + S32 ikIndex = findIKChain(name); + if (ikIndex < 0) + { + Con::errorf("TSShape::setIKChainMaxIterations: Could not find ikchain named '%s'", name.c_str()); + return false; + } + + TSShape::IKChain& ikChain = ikChains[ikIndex]; + + ikChain.maxIterations = maxIterations; + + return true; +} + +bool TSShape::setIKChainTarget(const String& name, const String& targetNode) +{ + S32 ikIndex = findIKChain(name); + if (ikIndex < 0) + { + Con::errorf("TSShape::setIKChainTarget: Could not find ikchain named '%s'", name.c_str()); + return false; + } + + TSShape::IKChain& ikChain = ikChains[ikIndex]; + + S32 nodeIndex = findNode(targetNode); + if (nodeIndex < 0) + { + Con::errorf("TSShape::setIKChainTarget: Could not find target node named '%s'", targetNode.c_str()); + return false; + } + + ikChain.targetIndex = nodeIndex; + + return true; +} //----------------------------------------------------------------------------- diff --git a/Engine/source/ts/tsShapeInstance.h b/Engine/source/ts/tsShapeInstance.h index 78ed5e641d..161f22bf57 100644 --- a/Engine/source/ts/tsShapeInstance.h +++ b/Engine/source/ts/tsShapeInstance.h @@ -584,6 +584,21 @@ class TSShapeInstance void setDirty(U32 dirty); void clearDirty(U32 dirty); + /// @name IKSolver functions + /// @{ + static Vector smFabrikBoneLengths; + static Vector smFabrikPositions; + static Vector smIKChainNodes; + void updateChildWorldTransforms(S32 node); + + bool solveCCD(TSShape::IKChain& chain); + bool calculateFabrikBoneLengths(); + void applyFabrik(TSShape::IKChain& chain); + void solveFabrikForward(const Point3F& target); + void solveFabrikBackward(const Point3F& root); + bool solveFrabrik(TSShape::IKChain& chain); + /// @} + //------------------------------------------------------------------------------------- // collision interface routines //------------------------------------------------------------------------------------- @@ -824,6 +839,7 @@ class TSThread U32 getSeqIndex() const { return sequence; } const TSSequence* getSequence() const { return &(mShapeInstance->mShape->sequences[sequence]); } const String& getSequenceName() const { return mShapeInstance->mShape->getSequenceName(sequence); } + const String& getIKChainName(S32 ikIndex) const { return mShapeInstance->mShape->getIKChainName(ikIndex); } S32 operator<(const TSThread &) const; };