├── .gitignore ├── Makefile ├── README.md ├── addTcSoftIkSolverCallbacks.cpp ├── addTcSoftIkSolverCallbacks.h ├── pluginMain.cpp ├── tcSoftIkSolverNew.cpp └── tcSoftIkSolverNew.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.sdf 2 | *.opensdf 3 | .project 4 | .cproject 5 | x64/ 6 | *.o 7 | Debug/ 8 | build/ 9 | *.exe 10 | *.msi -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | #- 2 | # ========================================================================== 3 | # Copyright 1995,2006,2008 Autodesk, Inc. All rights reserved. 4 | # 5 | # Use of this software is subject to the terms of the Autodesk 6 | # license agreement provided at the time of installation or download, 7 | # or which otherwise accompanies this software in either electronic 8 | # or hard copy form. 9 | # ========================================================================== 10 | #+ 11 | NAME = tcSoftIkSolver 12 | MAJOR = 1 13 | MINOR = 0.0 14 | VERSION = $(MAJOR).$(MINOR) 15 | 16 | LIBNAME = $(NAME).so 17 | 18 | C++ = g++412 19 | MAYAVERSION = 2014 20 | # Uncomment the following line on MIPSpro 7.2 compilers to supress 21 | # warnings about declarations in the X header files 22 | # WARNFLAGS = -woff 3322 23 | 24 | # When the following line is present, the linking of plugins will 25 | # occur much faster. If you link transitively (the default) then 26 | # a number of "ld Warning 85" messages will be printed by ld. 27 | # They are expected, and can be safely ignored. 28 | MAYA_LOCATION = /usr/autodesk/maya$(MAYAVERSION)-x64/ 29 | INSTALL_PATH = /home/alan/projects/deploy/maya/tcSoftIkSolver/1.0/plug-ins/$(MAYAVERSION)/linux 30 | CFLAGS = -m64 -pthread -pipe -D_BOOL -DLINUX -DREQUIRE_IOSTREAM -Wno-deprecated -fno-gnu-keywords -fPIC 31 | C++FLAGS = $(CFLAGS) $(WARNFLAGS) 32 | INCLUDES = -I. -I$(MAYA_LOCATION)/include -I/home/alan/projects/build/include/libxml2 -I/home/alan/projects/build/include -I/home/alan/projects/build/include/LicenseClient 33 | LD = $(C++) -shared $(C++FLAGS) 34 | LIBS = -L$(MAYA_LOCATION)/lib -lOpenMaya -lOpenMayaAnim -L/home/alan/projects/build/lib -lLicenseClient -lSockets -lssl -lcrypto -lpthread -ldl 35 | 36 | .SUFFIXES: .cpp .cc .o .so .c 37 | 38 | .cc.o: 39 | $(C++) -c $(INCLUDES) $(C++FLAGS) $< 40 | 41 | .cpp.o: 42 | $(C++) -c $(INCLUDES) $(C++FLAGS) $< 43 | 44 | .cc.i: 45 | $(C++) -E $(INCLUDES) $(C++FLAGS) $*.cc > $*.i 46 | 47 | .cc.so: 48 | -rm -f $@ 49 | $(LD) -o $@ $(INCLUDES) $< $(LIBS) 50 | 51 | .cpp.so: 52 | -rm -f $@ 53 | $(LD) -o $@ $(INCLUDES) $< $(LIBS) 54 | 55 | .o.so: 56 | -rm -f $@ 57 | $(LD) -o $@ $< $(LIBS) 58 | 59 | CPP_FILES := $(wildcard *.cpp) 60 | OBJS = $(notdir $(CPP_FILES:.cpp=.o)) 61 | 62 | all: $(LIBNAME) 63 | @echo "Done" 64 | mv $(LIBNAME) $(INSTALL_PATH) 65 | 66 | $(LIBNAME): $(OBJS) 67 | -rm -f $@ 68 | $(LD) -o $@ $(OBJS) $(LIBS) 69 | 70 | depend: 71 | makedepend $(INCLUDES) -I/usr/include/CC *.cc 72 | 73 | clean: 74 | -rm -f *.o *.so 75 | 76 | Clean: 77 | -rm -f *.o *.so *.bak 78 | 79 | install: all 80 | mv $(LIBNAME) $(INSTALL_PATH) 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Maya Soft IK Solver 2 | 3 | Our maya IK solver is an advanced solution for 2 bones setup. It fixes the annoying “pop” problem at full extension in the normal IK system. Moreover it implements a full controllable stretch algorithm and a “elbow slide” and “elbow lock” system needed by modern rigs. The advantage of our solver is that no extra nodes are needed and for this reason is faster than current solutions. It’s possible to update old rigs without creating new nodes or modifying the setup, just load our plugin and choose the tcSoftIKSolver from the ikHandle. 4 | 5 | Features: 6 | 7 | * Soft extension 8 | * Stretch 9 | * Elbow slide 10 | * Elbow lock 11 | 12 | If you are planning to use one of our tools in a studio, we would be grateful if you could let us know. 13 | 14 | 15 | ## How to use: 16 | 17 | Load the tcSoftIkSolver plug-in from the Maya plug-ins window. The plug-in will automatically add a tcSoftIkSolver node in the following cases: when a scene is opened, when a scene is created, when the plugin is loaded. 18 | 19 | Once the tcSoftIkSolver will be selected as “IK Solver” for an ikHandle, some attributes will be added to the same ikHandle: 20 | 21 | * Activate soft: enable/disable the soft extension. When this is disabled, the solver works like the normal maya ik, where there is the “pop” problem as the ik approaches the full extension. When this is enabled the velocity of the joints that approach the full extension position is damped by an exponential function controlled by the “soft distance” attribute. The solver starts to use this exponential function when the distance between the first joint and the ikHandle is equal to the full extension, less the “soft distance”. 22 | * Soft distance: Distance used by the exponential function of the “soft” algorithm to damp the extension of the chain 23 | * Activate stretch: enable/disable the stretch of the joint. When enabled the joints will be stretched in order to always reach the ikHandle position 24 | * Mid joint slide: This attribute add an offset to the middle joint in the chain, the offset is parallel to the vector between the start joint and the end joint. Only the middle joint of the chain is moved, while the start and end joint remain in the same position 25 | * Mid joint lock weight: This control the blend between the position of the mid joint computed by the nromal ik algorithm and the position given by the “Mid joint lock position” or by the pole vector if the attribute “Use pole vector as lock position” is checked 26 | * Mid joint lock position: This is the position used to constriant the position of the mid joint when the “Mid joint lock weight” attribute is greater than 0.0 27 | * Use pole vector as lock position: The pole vector is used instead the “Mid joint lock position” to constraint the mid joint 28 | * Mid/End joint scale: mid/end joint scale in rest position 29 | * Mid/End joint rotate order: mid/end joint rotate order in rest position 30 | * Mid/End joint rotate: mid/end joint rotation in rest position 31 | * Mid/End joint translate: mid/end joint translation in rest position 32 | * Mid/End joint orient: mid/end joint orient in rest position 33 | * Mid/End joint rotate axis: mid/end joint rotate axis in rest position 34 | * Mid/End joint parent inverse scale: mid/end joint parent scale in rest position 35 | 36 | 37 | ## Known limitations: 38 | 39 | * The Soft Ik solver works only with chains of 2 bones. 40 | * When using Viewport 2.0, the drawing of the joints may be incorrect. The legacy viewport doesn’t have this issue and it will always draw the joints correctly. 41 | * Due to a bug in the maya undo system when some constraint are attached directly or indirectly to the joints or to the ikHandle, sometimes maya doesn’t refresh the correct position when and undo is executed. We reported this bug to autodesk we’ll let you know when they will fix it. 42 | 43 | ## License 44 | 45 | This project is licensed under [the LGPL license](http://www.gnu.org/licenses/). 46 | 47 | ## Contact us 48 | 49 | Please feel free to contact us at support@toolchefs.com in case you would like contribute or simply have questions. 50 | 51 | ### ENJOY! 52 | -------------------------------------------------------------------------------- /addTcSoftIkSolverCallbacks.cpp: -------------------------------------------------------------------------------- 1 | #include "addTcSoftIkSolverCallbacks.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | 9 | MCallbackId addTcSoftIkSolverCallbacks::afterNewId; 10 | MCallbackId addTcSoftIkSolverCallbacks::afterOpenId; 11 | 12 | void *addTcSoftIkSolverCallbacks::creator() 13 | { 14 | return new addTcSoftIkSolverCallbacks; 15 | } 16 | 17 | void createIK2BsolverAfterNew(void *clientData) 18 | // 19 | // This method creates the ik2Bsolver after a File->New. 20 | // 21 | { 22 | MSelectionList selList; 23 | MGlobal::getActiveSelectionList( selList ); 24 | MGlobal::executeCommand("if(!`objExists tcSoftIkSolver`){createNode -n tcSoftIkSolver tcSoftIkSolver;};"); 25 | MGlobal::setActiveSelectionList( selList ); 26 | } 27 | 28 | void createIK2BsolverAfterOpen(void *clientData) 29 | // 30 | // This method creates the ik2Bsolver after a File->Open 31 | // if the ik2Bsolver does not exist in the loaded file. 32 | // 33 | { 34 | MSelectionList selList; 35 | MGlobal::getSelectionListByName("tcSoftIkSolver", selList); 36 | if (selList.length() == 0) { 37 | MGlobal::getActiveSelectionList( selList ); 38 | MGlobal::executeCommand("if(!`objExists tcSoftIkSolver`){createNode -n tcSoftIkSolver tcSoftIkSolver;};"); 39 | MGlobal::setActiveSelectionList( selList ); 40 | } 41 | } 42 | 43 | MStatus addTcSoftIkSolverCallbacks::doIt(const MArgList &args) 44 | // 45 | // This method adds the File->New and File->Open callbacks 46 | // used to recreate the ik2Bsolver. 47 | // 48 | { 49 | // Get the callback IDs so we can deregister them 50 | // when the plug-in is unloaded. 51 | afterNewId = MSceneMessage::addCallback(MSceneMessage::kAfterNew, 52 | createIK2BsolverAfterNew); 53 | afterOpenId = MSceneMessage::addCallback(MSceneMessage::kAfterOpen, 54 | createIK2BsolverAfterOpen); 55 | return MS::kSuccess; 56 | } 57 | -------------------------------------------------------------------------------- /addTcSoftIkSolverCallbacks.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | class addTcSoftIkSolverCallbacks : public MPxCommand 9 | { 10 | public: 11 | 12 | addTcSoftIkSolverCallbacks() {}; 13 | 14 | virtual MStatus doIt (const MArgList &); 15 | 16 | static void* creator(); 17 | 18 | // callback IDs for the solver callbacks 19 | static MCallbackId afterNewId; 20 | 21 | static MCallbackId afterOpenId; 22 | }; 23 | -------------------------------------------------------------------------------- /pluginMain.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (C) toolchefs 3 | // 4 | // File: pluginMain.cpp 5 | // 6 | // Author: Maya Plug-in Wizard 2.0 7 | // 8 | 9 | #include 10 | #include 11 | #include "tcSoftIkSolverNew.h" 12 | #include "addTcSoftIkSolverCallbacks.h" 13 | 14 | 15 | MStatus initializePlugin( MObject obj ) 16 | // 17 | // Description: 18 | // this method is called when the plug-in is loaded into Maya. It 19 | // registers all of the services that this plug-in provides with 20 | // Maya. 21 | // 22 | // Arguments: 23 | // obj - a handle to the plug-in object (use MFnPlugin to access it) 24 | // 25 | { 26 | MFnPlugin plugin( obj, "Toolchefs_tcSoftIkSolver", "1.0", "Any"); 27 | 28 | MStatus status = MStatus::kSuccess; 29 | MString errorString; 30 | 31 | status = plugin.registerNode("tcSoftIkSolver", 32 | TcSoftIkSolverNodeNew::id, 33 | &TcSoftIkSolverNodeNew::creator, 34 | &TcSoftIkSolverNodeNew::initialize, 35 | MPxNode::kIkSolverNode); 36 | if(status != MS::kSuccess) { 37 | status.perror("registerNode"); 38 | return status; 39 | } 40 | 41 | status = plugin.registerCommand("addTcSoftIkSolverCallbacks", 42 | addTcSoftIkSolverCallbacks::creator); 43 | if(status != MS::kSuccess) { 44 | status.perror("registerCommand"); 45 | return status; 46 | } 47 | 48 | MGlobal::executeCommand("addTcSoftIkSolverCallbacks;if(!`objExists tcSoftIkSolver`){createNode -n tcSoftIkSolver tcSoftIkSolver;};",false,false); 49 | return status; 50 | } 51 | 52 | MStatus uninitializePlugin( MObject obj ) 53 | // 54 | // Description: 55 | // this method is called when the plug-in is unloaded from Maya. It 56 | // deregisters all of the services that it was providing. 57 | // 58 | // Arguments: 59 | // obj - a handle to the plug-in object (use MFnPlugin to access it) 60 | // 61 | { 62 | MStatus status; 63 | MFnPlugin plugin( obj ); 64 | 65 | status = plugin.deregisterNode(TcSoftIkSolverNodeNew::id); 66 | if (!status) { 67 | status.perror("deregisterNode"); 68 | return status; 69 | } 70 | 71 | status = plugin.deregisterCommand("addTcSoftIkSolverCallbacks"); 72 | if (!status) { 73 | status.perror("deregisterCommand"); 74 | return status; 75 | } 76 | 77 | // Remove callbacks when plug-in is unloaded. 78 | MMessage::removeCallback(addTcSoftIkSolverCallbacks::afterNewId); 79 | MMessage::removeCallback(addTcSoftIkSolverCallbacks::afterOpenId); 80 | 81 | return status; 82 | } 83 | -------------------------------------------------------------------------------- /tcSoftIkSolverNew.cpp: -------------------------------------------------------------------------------- 1 | #include "tcSoftIkSolverNew.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #define kFloatEpsilon 1.0e-5F 23 | #define kDoubleEpsilon 1.0e-10 24 | #define kDoubleEpsilonSqr 1.0e-20 25 | #define kExtendedEpsilon kDoubleEpsilon 26 | 27 | MTypeId TcSoftIkSolverNodeNew::id(0x00122C00); 28 | 29 | inline bool equivalent(double x, double y, double fudge = kDoubleEpsilon) 30 | { 31 | return ((x > y) ? (x - y <= fudge) : (y - x <= fudge)); 32 | } 33 | 34 | bool isParallel(const MVector &thisVector, const MVector &otherVector, double tolerance = kDoubleEpsilon) 35 | { 36 | MVector v1, v2; 37 | v1 = thisVector.normal(); 38 | v2 = otherVector.normal(); 39 | double dotPrd = v1*v2; 40 | return (::equivalent(fabs(dotPrd), (double) 1.0, tolerance)); 41 | } 42 | 43 | MVector getBoneScales(MFnIkJoint& jointFn) 44 | { 45 | MPlug sxPlug = jointFn.findPlug("sx"); 46 | MPlug syPlug = jointFn.findPlug("sy"); 47 | MPlug szPlug = jointFn.findPlug("sz"); 48 | double sxValue = sxPlug.asDouble(); 49 | double syValue = syPlug.asDouble(); 50 | double szValue = szPlug.asDouble(); 51 | return MVector(sxValue, syValue, szValue); 52 | } 53 | 54 | TcSoftIkSolverNodeNew::TcSoftIkSolverNodeNew() 55 | : MPxIkSolverNode() 56 | { 57 | 58 | setSingleChainOnly(true); 59 | setUniqueSolution(true); 60 | setPositionOnly(true); 61 | setMaxIterations(true); 62 | } 63 | 64 | TcSoftIkSolverNodeNew::~TcSoftIkSolverNodeNew() {} 65 | 66 | void* TcSoftIkSolverNodeNew::creator() 67 | { 68 | return new TcSoftIkSolverNodeNew; 69 | } 70 | 71 | MStatus TcSoftIkSolverNodeNew::initialize() 72 | { 73 | return MS::kSuccess; 74 | } 75 | 76 | MString TcSoftIkSolverNodeNew::solverTypeName() const 77 | { 78 | return MString(kSolverType); 79 | } 80 | 81 | MStatus setPointValue(MObject attr, MObject node, MVector& value) 82 | { 83 | MPlug parentPlug(node, attr); 84 | 85 | if (parentPlug.isNull() && (parentPlug.numChildren()!=3)) 86 | return MS::kFailure; 87 | 88 | MPlug xPlug = parentPlug.child(0); 89 | MPlug yPlug = parentPlug.child(1); 90 | MPlug zPlug = parentPlug.child(2); 91 | 92 | xPlug.setValue(value.x); 93 | yPlug.setValue(value.y); 94 | zPlug.setValue(value.z); 95 | 96 | return MS::kSuccess; 97 | } 98 | 99 | MVector getPointValue(MPlug parentPlug) 100 | { 101 | MVector outVec; 102 | if (parentPlug.isNull() && (parentPlug.numChildren() != 3)) 103 | return outVec; 104 | 105 | MPlug xPlug = parentPlug.child(0); 106 | MPlug yPlug = parentPlug.child(1); 107 | MPlug zPlug = parentPlug.child(2); 108 | 109 | xPlug.getValue(outVec.x); 110 | yPlug.getValue(outVec.y); 111 | zPlug.getValue(outVec.z); 112 | 113 | return outVec; 114 | } 115 | 116 | MTransformationMatrix::RotationOrder getRotationOrder(short value) 117 | { 118 | switch (value) 119 | { 120 | case 0: 121 | return MTransformationMatrix::kInvalid; 122 | case 1: 123 | return MTransformationMatrix::kXYZ; //!< \nop 124 | case 2: 125 | return MTransformationMatrix::kYZX; //!< \nop 126 | case 3: 127 | return MTransformationMatrix::kZXY; //!< \nop 128 | case 4: 129 | return MTransformationMatrix::kXZY; //!< \nop 130 | case 5: 131 | return MTransformationMatrix::kYXZ; //!< \nop 132 | case 6: 133 | return MTransformationMatrix::kZYX; //!< \nop 134 | case 7: 135 | return MTransformationMatrix::kLast; //!< \nop 136 | default: 137 | return MTransformationMatrix::kInvalid; 138 | } 139 | } 140 | 141 | MEulerRotation::RotationOrder getEulerRotationOrder(short value) 142 | { 143 | switch (value) 144 | { 145 | case 0: 146 | return MEulerRotation::kXYZ; 147 | case 1: 148 | return MEulerRotation::kXYZ; //!< \nop 149 | case 2: 150 | return MEulerRotation::kYZX; //!< \nop 151 | case 3: 152 | return MEulerRotation::kZXY; //!< \nop 153 | case 4: 154 | return MEulerRotation::kXZY; //!< \nop 155 | case 5: 156 | return MEulerRotation::kYXZ; //!< \nop 157 | case 6: 158 | return MEulerRotation::kZYX; //!< \nop 159 | default: 160 | return MEulerRotation::kXYZ; 161 | } 162 | } 163 | 164 | short getShortRotationOrder(MTransformationMatrix::RotationOrder value) 165 | { 166 | 167 | if (MTransformationMatrix::kInvalid == value) 168 | return 0; 169 | if (MTransformationMatrix::kXYZ == value) 170 | return 1; 171 | if (MTransformationMatrix::kYZX == value) 172 | return 2; 173 | if (MTransformationMatrix::kZXY == value) 174 | return 3; 175 | if (MTransformationMatrix::kXZY == value) 176 | return 4; 177 | if (MTransformationMatrix::kYXZ == value) 178 | return 5; 179 | if (MTransformationMatrix::kZYX == value) 180 | return 6; 181 | if (MTransformationMatrix::kLast == value) 182 | return 7; 183 | return MTransformationMatrix::kInvalid; 184 | 185 | } 186 | 187 | 188 | 189 | MMatrix getMidRestLocalMatrix(MFnIkHandle& handleFn, MVector parentScale = MVector(1.0, 1.0, 1.0)) 190 | { 191 | MMatrix outMtx; 192 | MPlug plug = handleFn.findPlug("mjs"); 193 | MVector scale = getPointValue(plug); 194 | 195 | plug = handleFn.findPlug("mjr"); 196 | MVector rotation = getPointValue(plug); 197 | 198 | short value = 1; 199 | plug = handleFn.findPlug("mjro"); 200 | plug.getValue(value); 201 | MEulerRotation::RotationOrder rotOrder = getEulerRotationOrder(value); 202 | 203 | plug = handleFn.findPlug("mjt"); 204 | MVector translation = getPointValue(plug); 205 | 206 | MMatrix parentInverScale; 207 | parentInverScale[0][0] = parentScale.x; 208 | parentInverScale[1][1] = parentScale.y; 209 | parentInverScale[2][2] = parentScale.z; 210 | parentInverScale = parentInverScale.inverse(); 211 | 212 | plug = handleFn.findPlug("mjo"); 213 | MVector jointOrient = getPointValue(plug); 214 | 215 | plug = handleFn.findPlug("mjra"); 216 | MVector rotateAxis = getPointValue(plug); 217 | 218 | MTransformationMatrix mr1; 219 | mr1.rotateBy(MEulerRotation(rotation, rotOrder), MSpace::kTransform); 220 | 221 | MTransformationMatrix ms1; 222 | double scaleVec[3] = { scale.x, scale.y, scale.z }; 223 | ms1.setScale(scaleVec, MSpace::kTransform); 224 | 225 | MTransformationMatrix mt1; 226 | mt1.setTranslation(translation, MSpace::kTransform); 227 | 228 | MTransformationMatrix mjo1; 229 | mjo1.rotateBy(MEulerRotation(jointOrient, rotOrder), MSpace::kTransform); 230 | 231 | MTransformationMatrix mra1; 232 | mra1.rotateBy(MEulerRotation(rotateAxis, rotOrder), MSpace::kTransform); 233 | 234 | outMtx = ms1.asMatrix() * mra1.asMatrix() * mr1.asMatrix() * mjo1.asMatrix() * parentInverScale * mt1.asMatrix(); 235 | 236 | return outMtx; 237 | } 238 | 239 | MMatrix getEndRestLocalMatrix(MFnIkHandle& handleFn) 240 | { 241 | MMatrix outMtx; 242 | MPlug plug = handleFn.findPlug("ejs"); 243 | MVector scale = getPointValue(plug); 244 | 245 | plug = handleFn.findPlug("ejr"); 246 | MVector rotation = getPointValue(plug); 247 | 248 | short value = 1; 249 | plug = handleFn.findPlug("ejro"); 250 | plug.getValue(value); 251 | MEulerRotation::RotationOrder rotOrder = getEulerRotationOrder(value); 252 | 253 | plug = handleFn.findPlug("ejt"); 254 | MVector translation = getPointValue(plug); 255 | 256 | plug = handleFn.findPlug("ejis"); 257 | MVector parentScale = getPointValue(plug); 258 | MMatrix parentInverScale; 259 | parentInverScale[0][0] = parentScale.x; 260 | parentInverScale[1][1] = parentScale.y; 261 | parentInverScale[2][2] = parentScale.z; 262 | parentInverScale = parentInverScale.inverse(); 263 | 264 | plug = handleFn.findPlug("ejo"); 265 | MVector jointOrient = getPointValue(plug); 266 | 267 | plug = handleFn.findPlug("ejra"); 268 | MVector rotateAxis = getPointValue(plug); 269 | 270 | MTransformationMatrix mr1; 271 | mr1.rotateBy(MEulerRotation(rotation, rotOrder), MSpace::kTransform); 272 | 273 | MTransformationMatrix ms1; 274 | double scaleVec[3] = { scale.x, scale.y, scale.z }; 275 | ms1.setScale(scaleVec, MSpace::kTransform); 276 | 277 | MTransformationMatrix mt1; 278 | mt1.setTranslation(translation, MSpace::kTransform); 279 | 280 | MTransformationMatrix mjo1; 281 | mjo1.rotateBy(MEulerRotation(jointOrient, rotOrder), MSpace::kTransform); 282 | 283 | MTransformationMatrix mra1; 284 | mra1.rotateBy(MEulerRotation(rotateAxis, rotOrder), MSpace::kTransform); 285 | 286 | outMtx = ms1.asMatrix() * mra1.asMatrix() * mr1.asMatrix() * mjo1.asMatrix() * parentInverScale * mt1.asMatrix(); 287 | 288 | return outMtx; 289 | } 290 | 291 | MStatus TcSoftIkSolverNodeNew::preSolve() 292 | { 293 | MStatus status; 294 | setRotatePlane(true); 295 | setSingleChainOnly(true); 296 | setPositionOnly(true); 297 | 298 | MIkHandleGroup *handleGrp = handleGroup(); 299 | MObject handle = handleGrp->handle(0); 300 | MDagPath handlePath = MDagPath::getAPathTo(handle); 301 | MFnIkHandle handleFn(handlePath, &status); 302 | 303 | MDagPath effectorPath; 304 | handleFn.getEffector(effectorPath); 305 | MFnIkEffector effectorFn(effectorPath); 306 | effectorPath.pop(); 307 | MFnIkJoint midJointFn(effectorPath); 308 | 309 | MDagPath startJointPath; 310 | handleFn.getStartJoint(startJointPath); 311 | MFnIkJoint startJointFn(startJointPath); 312 | 313 | MDagPath lastJointPath = getEndJoint(handlePath); 314 | MFnIkJoint endJointFn(lastJointPath); 315 | 316 | if (!handleFn.hasAttribute("asft", &status)) 317 | { 318 | MFnNumericAttribute fnAttr; 319 | MObject attr = fnAttr.create("activateSoft", "asft", MFnNumericData::kBoolean, true); 320 | fnAttr.setKeyable(1); 321 | fnAttr.setWritable(1); 322 | fnAttr.setStorable(1); 323 | fnAttr.setReadable(1); 324 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 325 | } 326 | 327 | if (!handleFn.hasAttribute("sftd", &status)) 328 | { 329 | MFnNumericAttribute fnAttr; 330 | MObject attr = fnAttr.create("softDistance", "sftd", MFnNumericData::kDouble, 1.0); 331 | fnAttr.setKeyable(1); 332 | fnAttr.setWritable(1); 333 | fnAttr.setStorable(1); 334 | fnAttr.setReadable(1); 335 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 336 | } 337 | 338 | if (!handleFn.hasAttribute("astc", &status)) 339 | { 340 | MFnNumericAttribute fnAttr; 341 | MObject attr = fnAttr.create("activateStretch", "astc", MFnNumericData::kDouble, 1.0); 342 | fnAttr.setMin(0.0); 343 | fnAttr.setMax(1.0); 344 | fnAttr.setKeyable(1); 345 | fnAttr.setWritable(1); 346 | fnAttr.setStorable(1); 347 | fnAttr.setReadable(1); 348 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 349 | } 350 | 351 | if (!handleFn.hasAttribute("mjsld", &status)) 352 | { 353 | MFnNumericAttribute fnAttr; 354 | MObject attr = fnAttr.create("midJointSlide", "mjsld", MFnNumericData::kDouble, 0.0); 355 | fnAttr.setMax(1.0); 356 | fnAttr.setMin(-1.0); 357 | fnAttr.setKeyable(1); 358 | fnAttr.setWritable(1); 359 | fnAttr.setStorable(1); 360 | fnAttr.setReadable(1); 361 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 362 | } 363 | 364 | bool initRestPose = false; 365 | 366 | if (!handleFn.hasAttribute("mjlockw", &status)) 367 | { 368 | MFnNumericAttribute fnAttr; 369 | MObject attr = fnAttr.create("midJointLockWeight", "mjlockw", MFnNumericData::kDouble, 0.0); 370 | fnAttr.setMin(0.0); 371 | fnAttr.setMax(1.0); 372 | fnAttr.setKeyable(1); 373 | fnAttr.setWritable(1); 374 | fnAttr.setStorable(1); 375 | fnAttr.setReadable(1); 376 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 377 | } 378 | 379 | if (!handleFn.hasAttribute("mjlockp", &status)) 380 | { 381 | MFnNumericAttribute fnAttr; 382 | MObject attr = fnAttr.createPoint("midJointLockPosition", "mjlockp"); 383 | fnAttr.setKeyable(1); 384 | fnAttr.setWritable(1); 385 | fnAttr.setStorable(1); 386 | fnAttr.setReadable(1); 387 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 388 | } 389 | 390 | if (!handleFn.hasAttribute("plLock", &status)) 391 | { 392 | MFnNumericAttribute fnAttr; 393 | MObject attr = fnAttr.create("usePoleVectorAsLockPosition", "plLock", MFnNumericData::kBoolean, false); 394 | fnAttr.setKeyable(1); 395 | fnAttr.setWritable(1); 396 | fnAttr.setStorable(1); 397 | fnAttr.setReadable(1); 398 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 399 | } 400 | 401 | if (!handleFn.hasAttribute("mjs", &status)) 402 | { 403 | MFnNumericAttribute fnAttr; 404 | MObject attr = fnAttr.createPoint("midJointScale", "mjs"); 405 | fnAttr.setKeyable(1); 406 | fnAttr.setWritable(1); 407 | fnAttr.setStorable(1); 408 | fnAttr.setReadable(1); 409 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 410 | 411 | double scale[3] = { 0.0, 0.0, 0.0 }; 412 | midJointFn.getScale(scale); 413 | setPointValue(attr, handleFn.object(), MVector(scale[0], scale[1], scale[2])); 414 | } 415 | MTransformationMatrix::RotationOrder rotateOrder = MTransformationMatrix::kXYZ; 416 | if (!handleFn.hasAttribute("mjro", &status)) 417 | { 418 | MFnNumericAttribute fnAttr; 419 | MObject attr = fnAttr.create("midJointRotateOrder", "mjro", MFnNumericData::kShort, 1); 420 | fnAttr.setKeyable(1); 421 | fnAttr.setWritable(1); 422 | fnAttr.setStorable(1); 423 | fnAttr.setReadable(1); 424 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 425 | 426 | MPlug plug(handleFn.object(), attr); 427 | rotateOrder = midJointFn.rotationOrder(); 428 | plug.setValue(getShortRotationOrder(rotateOrder)); 429 | } 430 | 431 | if (!handleFn.hasAttribute("mjr", &status)) 432 | { 433 | MFnNumericAttribute fnAttr; 434 | MObject attr = fnAttr.createPoint("midJointRotate", "mjr"); 435 | fnAttr.setKeyable(1); 436 | fnAttr.setWritable(1); 437 | fnAttr.setStorable(1); 438 | fnAttr.setReadable(1); 439 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 440 | 441 | double rotate[3] = { 0.0, 0.0, 0.0 }; 442 | midJointFn.getRotation(rotate, rotateOrder); 443 | setPointValue(attr, handleFn.object(), MVector(rotate[0], rotate[1], rotate[2])); 444 | } 445 | 446 | if (!handleFn.hasAttribute("mjt", &status)) 447 | { 448 | MFnNumericAttribute fnAttr; 449 | MObject attr = fnAttr.createPoint("midJointTranslate", "mjt"); 450 | fnAttr.setKeyable(1); 451 | fnAttr.setWritable(1); 452 | fnAttr.setStorable(1); 453 | fnAttr.setReadable(1); 454 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 455 | 456 | setPointValue(attr, handleFn.object(), midJointFn.getTranslation(MSpace::kTransform)); 457 | } 458 | 459 | if (!handleFn.hasAttribute("mjo", &status)) 460 | { 461 | MFnNumericAttribute fnAttr; 462 | 463 | MObject attr = fnAttr.createPoint("midJointOrient", "mjo"); 464 | fnAttr.setKeyable(1); 465 | fnAttr.setWritable(1); 466 | fnAttr.setStorable(1); 467 | fnAttr.setReadable(1); 468 | 469 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 470 | 471 | MQuaternion orientation; 472 | midJointFn.getOrientation(orientation); 473 | MEulerRotation rot = orientation.asEulerRotation(); 474 | setPointValue(attr, handleFn.object(), MVector(rot.x, rot.y, rot.z)); 475 | } 476 | 477 | if (!handleFn.hasAttribute("mjra", &status)) 478 | { 479 | MFnNumericAttribute fnAttr; 480 | MFnCompoundAttribute cAttr; 481 | MObject attr = fnAttr.createPoint("midJointRotateAxis", "mjra"); 482 | fnAttr.setKeyable(1); 483 | fnAttr.setWritable(1); 484 | fnAttr.setStorable(1); 485 | fnAttr.setReadable(1); 486 | 487 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 488 | 489 | MQuaternion orientation; 490 | midJointFn.getScaleOrientation(orientation); 491 | MEulerRotation rot = orientation.asEulerRotation(); 492 | setPointValue(attr, handleFn.object(), MVector(rot.x, rot.y, rot.z)); 493 | 494 | } 495 | 496 | if (!handleFn.hasAttribute("mjis", &status)) 497 | { 498 | MFnNumericAttribute fnAttr; 499 | MObject attr = fnAttr.createPoint("midJointParentInverseScale", "mjis"); 500 | fnAttr.setKeyable(1); 501 | fnAttr.setWritable(1); 502 | fnAttr.setStorable(1); 503 | fnAttr.setReadable(1); 504 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 505 | 506 | double scale[3] = { 0.0, 0.0, 0.0 }; 507 | startJointFn.getScale(scale); 508 | setPointValue(attr, handleFn.object(), MVector(scale[0], scale[1], scale[2])); 509 | } 510 | 511 | // end joint 512 | if (!handleFn.hasAttribute("ejs", &status)) 513 | { 514 | MFnNumericAttribute fnAttr; 515 | MObject attr = fnAttr.createPoint("endJointScale", "ejs"); 516 | fnAttr.setKeyable(1); 517 | fnAttr.setWritable(1); 518 | fnAttr.setStorable(1); 519 | fnAttr.setReadable(1); 520 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 521 | 522 | double scale[3] = { 0.0, 0.0, 0.0 }; 523 | endJointFn.getScale(scale); 524 | setPointValue(attr, handleFn.object(), MVector(scale[0], scale[1], scale[2])); 525 | } 526 | 527 | if (!handleFn.hasAttribute("ejro", &status)) 528 | { 529 | MFnNumericAttribute fnAttr; 530 | MObject attr = fnAttr.create("endJointRotateOrder", "ejro", MFnNumericData::kShort, 1); 531 | fnAttr.setKeyable(1); 532 | fnAttr.setWritable(1); 533 | fnAttr.setStorable(1); 534 | fnAttr.setReadable(1); 535 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 536 | 537 | MPlug plug(handleFn.object(), attr); 538 | rotateOrder = endJointFn.rotationOrder(); 539 | plug.setValue(getShortRotationOrder(rotateOrder)); 540 | } 541 | 542 | if (!handleFn.hasAttribute("ejr", &status)) 543 | { 544 | MFnNumericAttribute fnAttr; 545 | MObject attr = fnAttr.createPoint("endJointRotate", "ejr"); 546 | fnAttr.setKeyable(1); 547 | fnAttr.setWritable(1); 548 | fnAttr.setStorable(1); 549 | fnAttr.setReadable(1); 550 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 551 | 552 | double rotate[3] = { 0.0, 0.0, 0.0 }; 553 | endJointFn.getRotation(rotate, rotateOrder); 554 | setPointValue(attr, handleFn.object(), MVector(rotate[0], rotate[1], rotate[2])); 555 | } 556 | 557 | if (!handleFn.hasAttribute("ejt", &status)) 558 | { 559 | MFnNumericAttribute fnAttr; 560 | MObject attr = fnAttr.createPoint("endJointTranslate", "ejt"); 561 | fnAttr.setKeyable(1); 562 | fnAttr.setWritable(1); 563 | fnAttr.setStorable(1); 564 | fnAttr.setReadable(1); 565 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 566 | 567 | setPointValue(attr, handleFn.object(), endJointFn.getTranslation(MSpace::kTransform)); 568 | } 569 | 570 | if (!handleFn.hasAttribute("ejo", &status)) 571 | { 572 | MFnNumericAttribute fnAttr; 573 | 574 | MObject attr = fnAttr.createPoint("endJointOrient", "ejo"); 575 | fnAttr.setKeyable(1); 576 | fnAttr.setWritable(1); 577 | fnAttr.setStorable(1); 578 | fnAttr.setReadable(1); 579 | 580 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 581 | 582 | MQuaternion orientation; 583 | endJointFn.getOrientation(orientation); 584 | MEulerRotation rot = orientation.asEulerRotation(); 585 | setPointValue(attr, handleFn.object(), MVector(rot.x, rot.y, rot.z)); 586 | } 587 | 588 | if (!handleFn.hasAttribute("ejra", &status)) 589 | { 590 | MFnNumericAttribute fnAttr; 591 | MFnCompoundAttribute cAttr; 592 | MObject attr = fnAttr.createPoint("endJointRotateAxis", "ejra"); 593 | fnAttr.setKeyable(1); 594 | fnAttr.setWritable(1); 595 | fnAttr.setStorable(1); 596 | fnAttr.setReadable(1); 597 | 598 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 599 | 600 | MQuaternion orientation; 601 | endJointFn.getScaleOrientation(orientation); 602 | MEulerRotation rot = orientation.asEulerRotation(); 603 | setPointValue(attr, handleFn.object(), MVector(rot.x, rot.y, rot.z)); 604 | 605 | } 606 | 607 | if (!handleFn.hasAttribute("ejis", &status)) 608 | { 609 | MFnNumericAttribute fnAttr; 610 | MObject attr = fnAttr.createPoint("endJointParentInverseScale", "ejis"); 611 | fnAttr.setKeyable(1); 612 | fnAttr.setWritable(1); 613 | fnAttr.setStorable(1); 614 | fnAttr.setReadable(1); 615 | handleFn.addAttribute(attr, MFnDependencyNode::kLocalDynamicAttr); 616 | 617 | double scale[3] = { 0.0, 0.0, 0.0 }; 618 | midJointFn.getScale(scale); 619 | setPointValue(attr, handleFn.object(), MVector(scale[0], scale[1], scale[2])); 620 | } 621 | 622 | return MS::kSuccess; 623 | } 624 | 625 | MStatus TcSoftIkSolverNodeNew::doSolve() 626 | { 627 | MStatus stat; 628 | // Handle Group 629 | // 630 | MIkHandleGroup * handle_group = handleGroup(); 631 | if (NULL == handle_group) { 632 | return MS::kFailure; 633 | } 634 | 635 | // Handle 636 | // 637 | // For single chain types of solvers, get the 0th handle. 638 | // Single chain solvers are solvers which act on one handle only, 639 | // i.e. the handle group for a single chain solver 640 | // has only one handle 641 | // 642 | MObject handle = handle_group->handle(0); 643 | MDagPath handlePath = MDagPath::getAPathTo(handle); 644 | MFnIkHandle handleFn(handlePath, &stat); 645 | 646 | // Effector 647 | // 648 | MDagPath effectorPath; 649 | handleFn.getEffector(effectorPath); 650 | MFnIkEffector effectorFn(effectorPath); 651 | 652 | // Mid Joint 653 | // 654 | effectorPath.pop(); 655 | MFnIkJoint midJointFn(effectorPath); 656 | 657 | // Start Joint 658 | // 659 | MDagPath startJointPath; 660 | handleFn.getStartJoint(startJointPath); 661 | MFnIkJoint startJointFn(startJointPath); 662 | 663 | MDagPath lastJointPath = getEndJoint(handlePath); 664 | MFnIkJoint endJointFn(lastJointPath); 665 | 666 | MPlug sftdPlug = handleFn.findPlug("sftd"); 667 | double m_softDistance = sftdPlug.asDouble(); 668 | 669 | MPlug doSoftPlug = handleFn.findPlug("asft"); 670 | double m_doSoft = doSoftPlug.asBool(); 671 | 672 | MPlug doStretchPlug = handleFn.findPlug("astc"); 673 | double m_doStretch = doStretchPlug.asDouble(); 674 | 675 | MPlug elbowSlidePlug = handleFn.findPlug("mjsld"); 676 | double m_elbowSlide = elbowSlidePlug.asDouble(); 677 | 678 | MPlug elbowLockWeigthPlug = handleFn.findPlug("mjlockw"); 679 | double m_elbowLockWeigth = elbowLockWeigthPlug.asDouble(); 680 | 681 | 682 | double scale[3] = { 1.0, 1.0, 1.0 }; 683 | startJointFn.getScale(scale); 684 | MMatrix midJointStartMatrix = getMidRestLocalMatrix(handleFn, MVector(scale[0], scale[1], scale[2])); 685 | MMatrix endJointStartMatrix = getEndRestLocalMatrix(handleFn); 686 | 687 | MMatrix midWorldMatrix = midJointStartMatrix * startJointPath.inclusiveMatrix(); 688 | MMatrix endWorldMatrix = endJointStartMatrix * midWorldMatrix; 689 | 690 | MPoint handlePos = handleFn.rotatePivot(MSpace::kWorld); 691 | //MPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld); 692 | //MPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld); 693 | MPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld); 694 | MPoint effectorPos(endWorldMatrix[3][0], endWorldMatrix[3][1], endWorldMatrix[3][2]); 695 | MPoint midJointPos(midWorldMatrix[3][0], midWorldMatrix[3][1], midWorldMatrix[3][2]); 696 | 697 | 698 | // slide joint 699 | MPoint slideMidPosition; 700 | MVector globalVec = effectorPos - startJointPos; 701 | MVector midVec = midJointPos - startJointPos; 702 | MVector endVec = effectorPos - midJointPos; 703 | MVector dispVec; 704 | if (m_elbowSlide < 0.0) 705 | { 706 | dispVec = (globalVec.normal() * midVec.normal()) * m_elbowSlide * midVec.length() * globalVec.normal(); 707 | } 708 | else 709 | { 710 | dispVec = (globalVec.normal() * endVec.normal()) * m_elbowSlide * endVec.length() * globalVec.normal(); 711 | } 712 | 713 | 714 | MVector vector1 = midVec + dispVec; 715 | double newMidLength = vector1.length(); 716 | MVector vector2 = globalVec - vector1; 717 | double newEndLength = vector2.length(); 718 | 719 | double newStartAngle = globalVec.normal().angle(vector1.normal()); 720 | double newMidAngle = vector1.normal().angle(vector2.normal()); 721 | 722 | double oldStartAngle = globalVec.normal().angle((midJointPos - startJointPos).normal()); 723 | double oldMidAngle = (midJointPos - startJointPos).normal().angle((effectorPos - midJointPos).normal()); 724 | 725 | MVector startCross = globalVec.normal() ^ (midJointPos - startJointPos).normal(); 726 | startCross.normalize(); 727 | MQuaternion qStartElbow(newStartAngle - oldStartAngle, startCross); 728 | 729 | MVector midCross = (midJointPos - startJointPos).normal() ^ (effectorPos - midJointPos).normal(); 730 | midCross.normalize(); 731 | MQuaternion qMidElbow(newMidAngle - oldMidAngle, midCross); 732 | 733 | //midJointFn.rotateBy(qMidElbow, MSpace::kWorld); 734 | //startJointFn.rotateBy(qStartElbow, MSpace::kWorld); 735 | 736 | MPlug plug2 = handleFn.findPlug("ejt"); 737 | MVector effectorTranslation2 = getPointValue(plug2); 738 | 739 | plug2 = handleFn.findPlug("mjt"); 740 | MVector midTranslation2 = getPointValue(plug2); 741 | 742 | MVector midTranslationOffset = midTranslation2; 743 | MVector effectorTranslationOffset = effectorTranslation2; 744 | midTranslationOffset = midTranslation2 - midTranslation2.normal() * newMidLength; 745 | effectorTranslationOffset = effectorTranslation2 - effectorTranslation2.normal() * newEndLength; 746 | //midJointFn.setTranslation(midTranslation, MSpace::kTransform); 747 | //endJointFn.setTranslation(effectorTranslation, MSpace::kTransform); 748 | 749 | // end slide joint 750 | 751 | 752 | 753 | 754 | MVector poleVector = poleVectorFromHandle(handlePath); 755 | poleVector *= handlePath.exclusiveMatrix(); 756 | double twistValue = twistFromHandle(handlePath); 757 | 758 | MQuaternion qStart, qMid; 759 | 760 | // vector from startJoint to midJoint 761 | //MVector vector1 = midJointPos - startJointPos; 762 | // vector from midJoint to effector 763 | //MVector vector2 = effectorPos - midJointPos; 764 | // vector from startJoint to handle 765 | MVector vectorH = handlePos - startJointPos; 766 | // vector from startJoint to effector 767 | MVector vectorE = effectorPos - startJointPos; 768 | 769 | //double startScale2[3] = {m_initStartScale[0] * m_stretchRatio, m_initStartScale[1], m_initStartScale[2]}; 770 | //double midScale2[3] = {m_initMidScale[0] * m_stretchRatio, m_initMidScale[1], m_initMidScale[2]}; 771 | 772 | bool doingSoft = false; 773 | if ((m_doSoft) && (m_softDistance > 0.0) && (vectorH.length() > ((vector1.length() + vector2.length()) - m_softDistance))) 774 | { 775 | double dChain = (vector1.length() + vector2.length()); 776 | double dSoft = m_softDistance; 777 | double dA = dChain - m_softDistance; 778 | double x = vectorH.length(); 779 | vectorH = vectorH.normal() * (dA + dSoft * (1.0 - exp(-(x - dA) / dSoft))); 780 | doingSoft = true; 781 | } 782 | 783 | // lengths of those vectors 784 | double length1 = vector1.length(); 785 | double length2 = vector2.length(); 786 | double lengthH = vectorH.length(); 787 | // component of the vector1 orthogonal to the vectorE 788 | MVector vectorO = 789 | vector1 - vectorE*((vector1*vectorE) / (vectorE*vectorE)); 790 | 791 | ////////////////////////////////////////////////////////////////// 792 | // calculate q12 which solves for the midJoint rotation 793 | ////////////////////////////////////////////////////////////////// 794 | // angle between vector1 and vector2 795 | double vectorAngle12 = vector1.angle(vector2); 796 | // vector orthogonal to vector1 and 2 797 | MVector vectorCross12 = vector1^vector2; 798 | double lengthHsquared = lengthH*lengthH; 799 | // angle for arm extension 800 | double cos_theta = 801 | (lengthHsquared - length1*length1 - length2*length2) 802 | / (2 * length1*length2); 803 | if (cos_theta > 1) 804 | cos_theta = 1; 805 | else if (cos_theta < -1) 806 | cos_theta = -1; 807 | double theta = acos(cos_theta); 808 | // quaternion for arm extension 809 | MQuaternion q12(theta - vectorAngle12, vectorCross12); 810 | 811 | ////////////////////////////////////////////////////////////////// 812 | // calculate qEH which solves for effector rotating onto the handle 813 | ////////////////////////////////////////////////////////////////// 814 | // vector2 with quaternion q12 applied 815 | vector2 = vector2.rotateBy(q12); 816 | // vectorE with quaternion q12 applied 817 | vectorE = vector1 + vector2; 818 | // quaternion for rotating the effector onto the handle 819 | MQuaternion qEH(vectorE, vectorH); 820 | 821 | ////////////////////////////////////////////////////////////////// 822 | // calculate qNP which solves for the rotate plane 823 | ////////////////////////////////////////////////////////////////// 824 | // vector1 with quaternion qEH applied 825 | vector1 = vector1.rotateBy(qEH); 826 | if (vector1.isParallel(vectorH)) 827 | // singular case, use orthogonal component instead 828 | vector1 = vectorO.rotateBy(qEH); 829 | // quaternion for rotate plane 830 | MQuaternion qNP; 831 | if (!isParallel(poleVector, vectorH) && (lengthHsquared != 0)) { 832 | // component of vector1 orthogonal to vectorH 833 | MVector vectorN = 834 | vector1 - vectorH*((vector1*vectorH) / lengthHsquared); 835 | // component of pole vector orthogonal to vectorH 836 | MVector vectorP = 837 | poleVector - vectorH*((poleVector*vectorH) / lengthHsquared); 838 | double dotNP = (vectorN*vectorP) / (vectorN.length()*vectorP.length()); 839 | if (absoluteValue(dotNP + 1.0) < kEpsilon) { 840 | // singular case, rotate halfway around vectorH 841 | MQuaternion qNP1(kPi, vectorH); 842 | qNP = qNP1; 843 | } 844 | else { 845 | MQuaternion qNP2(vectorN, vectorP); 846 | qNP = qNP2; 847 | } 848 | } 849 | 850 | ////////////////////////////////////////////////////////////////// 851 | // calculate qTwist which adds the twist 852 | ////////////////////////////////////////////////////////////////// 853 | MQuaternion qTwist(twistValue, vectorH); 854 | 855 | // quaternion for the mid joint 856 | qMid = q12; 857 | // concatenate the quaternions for the start joint 858 | qStart = qEH*qNP*qTwist; 859 | 860 | MVector midRotAxis; 861 | double midRot = 0.0; 862 | qMid.getAxisAngle(midRotAxis, midRot); 863 | midRotAxis = midRotAxis * midWorldMatrix.inverse(); 864 | 865 | MPlug plug = handleFn.findPlug("mjr"); 866 | MVector rotation = getPointValue(plug); 867 | 868 | short value = 1; 869 | plug = handleFn.findPlug("mjro"); 870 | plug.getValue(value); 871 | MEulerRotation::RotationOrder rotOrder = getEulerRotationOrder(value); 872 | MEulerRotation mRot(rotation, rotOrder); 873 | 874 | //qMidElbow 875 | MVector midElbowRotAxis; 876 | double midElbowRot = 0.0; 877 | qMidElbow.getAxisAngle(midElbowRotAxis, midElbowRot); 878 | midElbowRotAxis = midElbowRotAxis * midWorldMatrix.inverse(); 879 | 880 | midJointFn.setRotation(mRot.asQuaternion() * MQuaternion(midElbowRot, midElbowRotAxis) * MQuaternion(midRot, midRotAxis), MSpace::kTransform); 881 | startJointFn.rotateBy(qStart * qStartElbow, MSpace::kWorld); 882 | 883 | 884 | //midJointFn.setRotation(mRot.asQuaternion() * MQuaternion(midRot, midRotAxis), MSpace::kTransform); 885 | //startJointFn.rotateBy(qStart, MSpace::kWorld); 886 | 887 | 888 | ////////////////////////// 889 | // Stretch 890 | ///////////////////////// 891 | plug = handleFn.findPlug("ejt"); 892 | MVector effectorTranslation = getPointValue(plug) -effectorTranslationOffset; 893 | 894 | plug = handleFn.findPlug("mjt"); 895 | MVector midTranslation = getPointValue(plug) -midTranslationOffset; 896 | 897 | double m_stretchRatio = 1.0; 898 | double ratio = vectorH.length() / vectorE.length(); 899 | MVector vectorH2 = handlePos - startJointPos; 900 | if (doingSoft) 901 | { 902 | ratio = vectorH2.length() / vectorH.length(); 903 | } 904 | if ((m_doStretch > 0.0001) && (ratio > 1.0)) 905 | { 906 | m_stretchRatio = 1.0 + (ratio - 1.0) * m_doStretch; 907 | } 908 | else 909 | { 910 | m_stretchRatio = 1.0; 911 | } 912 | 913 | effectorTranslation = effectorTranslation * m_stretchRatio; 914 | midTranslation = midTranslation * m_stretchRatio; 915 | 916 | endJointFn.setTranslation(effectorTranslation, MSpace::kTransform); 917 | midJointFn.setTranslation(midTranslation, MSpace::kTransform); 918 | 919 | ///////////////////////// 920 | // Elbow lock 921 | ///////////////////////// 922 | if (m_elbowLockWeigth > 0.0) 923 | { 924 | 925 | MVector m_elbowPos; 926 | MPlug elbowPosmjlockplug = handleFn.findPlug("plLock"); 927 | if (elbowPosmjlockplug.asBool()) 928 | { 929 | MVector poleVector = poleVectorFromHandle(handlePath); 930 | poleVector *= handlePath.exclusiveMatrix(); 931 | m_elbowPos = startJointPos + poleVector; 932 | } 933 | else 934 | { 935 | MPlug elbowLockPosPlug = handleFn.findPlug("mjlockp"); 936 | MPlug pXPlug = elbowLockPosPlug.child(0); 937 | MPlug pYPlug = elbowLockPosPlug.child(1); 938 | MPlug pZPlug = elbowLockPosPlug.child(2); 939 | 940 | m_elbowPos.x = pXPlug.asDouble(); 941 | m_elbowPos.y = pYPlug.asDouble(); 942 | m_elbowPos.z = pZPlug.asDouble(); 943 | } 944 | 945 | MPlug endJointRestPosePlug = handleFn.findPlug("ejrp"); 946 | MPlug endXPlug = endJointRestPosePlug.child(0); 947 | MPlug endYPlug = endJointRestPosePlug.child(1); 948 | MPlug endZPlug = endJointRestPosePlug.child(2); 949 | double endLength = MVector(endXPlug.asDouble(), endYPlug.asDouble(), endZPlug.asDouble()).length(); 950 | 951 | MPoint handlePos3 = handleFn.rotatePivot(MSpace::kWorld); 952 | MPoint effectorPos3 = effectorFn.rotatePivot(MSpace::kWorld); 953 | MPoint midJointPos3 = midJointFn.rotatePivot(MSpace::kWorld); 954 | MPoint startJointPos3 = startJointFn.rotatePivot(MSpace::kWorld); 955 | 956 | MVector newFirstVector = m_elbowPos - startJointPos3 + (1.0 - m_elbowLockWeigth) * (midJointPos3 - m_elbowPos); 957 | MVector currVec1 = midJointPos3 - startJointPos3; 958 | MQuaternion newQ1(currVec1.normal(), newFirstVector.normal()); 959 | 960 | startJointFn.rotateBy(newQ1, MSpace::kWorld); 961 | midJointFn.setTranslation(m_elbowPos + (1.0 - m_elbowLockWeigth) * (midJointPos3 - m_elbowPos), MSpace::kWorld); 962 | 963 | handlePos3 = handleFn.rotatePivot(MSpace::kWorld); 964 | effectorPos3 = endJointFn.rotatePivot(MSpace::kWorld); 965 | midJointPos3 = midJointFn.rotatePivot(MSpace::kWorld); 966 | startJointPos3 = startJointFn.rotatePivot(MSpace::kWorld); 967 | 968 | MVector newSecondVector = handlePos3 - midJointPos3; 969 | MVector currVec2 = effectorPos3 - midJointPos3; 970 | MQuaternion newQ2(currVec2.normal(), newSecondVector.normal()); 971 | midJointFn.rotateBy(newQ2, MSpace::kWorld); 972 | endJointFn.setTranslation(endJointFn.translation(MSpace::kTransform).normal() * (endLength + (m_doStretch * (newSecondVector.length() - endLength))), MSpace::kTransform); 973 | } 974 | 975 | 976 | return MS::kSuccess; 977 | } 978 | 979 | MStatus TcSoftIkSolverNodeNew::postSolve(MStatus status) 980 | { 981 | 982 | 983 | return MS::kSuccess; 984 | } 985 | 986 | 987 | MVector TcSoftIkSolverNodeNew::poleVectorFromHandle(const MDagPath &handlePath) 988 | // 989 | // This method returns the pole vector of the IK handle. 990 | // 991 | { 992 | MStatus stat; 993 | MFnIkHandle handleFn(handlePath, &stat); 994 | MPlug pvxPlug = handleFn.findPlug("pvx"); 995 | MPlug pvyPlug = handleFn.findPlug("pvy"); 996 | MPlug pvzPlug = handleFn.findPlug("pvz"); 997 | double pvxValue, pvyValue, pvzValue; 998 | pvxPlug.getValue(pvxValue); 999 | pvyPlug.getValue(pvyValue); 1000 | pvzPlug.getValue(pvzValue); 1001 | MVector poleVector(pvxValue, pvyValue, pvzValue); 1002 | return poleVector; 1003 | } 1004 | 1005 | double TcSoftIkSolverNodeNew::twistFromHandle(const MDagPath &handlePath) 1006 | // 1007 | // This method returns the twist of the IK handle. 1008 | // 1009 | { 1010 | MStatus stat; 1011 | MFnIkHandle handleFn(handlePath, &stat); 1012 | MPlug twistPlug = handleFn.findPlug("twist"); 1013 | double twistValue; 1014 | twistPlug.getValue(twistValue); 1015 | return twistValue; 1016 | } 1017 | 1018 | MDagPath TcSoftIkSolverNodeNew::getEndJoint(const MDagPath &handlePath) 1019 | { 1020 | MStatus status; 1021 | 1022 | MFnIkHandle handleFn(handlePath, &status); 1023 | // Effector 1024 | // 1025 | MDagPath effectorPath; 1026 | handleFn.getEffector(effectorPath); 1027 | MFnIkEffector effectorFn(effectorPath); 1028 | 1029 | MPlug effectorPlug = effectorFn.findPlug("tx"); 1030 | MPlugArray plugArr; 1031 | MObject jointNode; 1032 | if (effectorPlug.connectedTo(plugArr, true, false, &status) && (plugArr.length() > 0)) 1033 | { 1034 | if (plugArr[0].node().hasFn(MFn::kJoint)) 1035 | { 1036 | jointNode = plugArr[0].node(); 1037 | } 1038 | } 1039 | else 1040 | { 1041 | effectorPlug = effectorFn.findPlug("ty"); 1042 | if (effectorPlug.connectedTo(plugArr, true, false, &status) && (plugArr.length() > 0)) 1043 | { 1044 | if (plugArr[0].node().hasFn(MFn::kJoint)) 1045 | { 1046 | jointNode = plugArr[0].node(); 1047 | } 1048 | } 1049 | else 1050 | { 1051 | effectorPlug = effectorFn.findPlug("tz"); 1052 | if (effectorPlug.connectedTo(plugArr, true, false, &status) && (plugArr.length() > 0)) 1053 | { 1054 | if (plugArr[0].node().hasFn(MFn::kJoint)) 1055 | { 1056 | jointNode = plugArr[0].node(); 1057 | } 1058 | } 1059 | } 1060 | } 1061 | // slide joint 1062 | MDagPath lastJointPath; 1063 | effectorPath.pop(); 1064 | for (unsigned int i = 0; i < effectorPath.childCount(); ++i) 1065 | { 1066 | MDagPath currJointPath = effectorPath; 1067 | MObject child = effectorPath.child(i); 1068 | currJointPath.push(child); 1069 | if (child.hasFn(MFn::kJoint)) 1070 | { 1071 | lastJointPath = currJointPath; 1072 | if (lastJointPath.node() == jointNode) 1073 | { 1074 | break; 1075 | } 1076 | } 1077 | } 1078 | 1079 | return lastJointPath; 1080 | } -------------------------------------------------------------------------------- /tcSoftIkSolverNew.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define MAX_ITERATIONS 1 11 | #define kSolverType "softIkSolverNode" 12 | #define kPi 3.14159265358979323846264338327950 13 | #define kEpsilon 1.0e-5 14 | #define absoluteValue(x) ((x) < 0 ? (-(x)) : (x)) 15 | 16 | class TcSoftIkSolverNodeNew : public MPxIkSolverNode { 17 | public: 18 | TcSoftIkSolverNodeNew(); 19 | 20 | virtual ~TcSoftIkSolverNodeNew(); 21 | 22 | virtual MStatus preSolve(); 23 | 24 | virtual MStatus doSolve(); 25 | 26 | virtual MStatus postSolve(MStatus stat); 27 | 28 | virtual MString solverTypeName() const; 29 | 30 | static void* creator(); 31 | 32 | static MStatus initialize(); 33 | 34 | static MTypeId id; 35 | 36 | 37 | private: 38 | 39 | MStatus doSimpleSolver(); 40 | 41 | MVector poleVectorFromHandle(const MDagPath &handlePath); 42 | 43 | MDagPath getEndJoint(const MDagPath &handlePath); 44 | 45 | double twistFromHandle(const MDagPath &handlePath); 46 | 47 | }; 48 | --------------------------------------------------------------------------------