diff --git a/SkeletonEditingPlugin.cc b/SkeletonEditingPlugin.cc index 95b03f66557be3560fceb52173f0f2e3556b9bdf..1a322ce47fdb212172ea554cd553aa6f964e7477 100644 --- a/SkeletonEditingPlugin.cc +++ b/SkeletonEditingPlugin.cc @@ -617,6 +617,88 @@ void SkeletonEditingPlugin::placeManip(QMouseEvent * _event) { } } +//------------------------------------------------------------------------------ +/* + * \brief move a joint using inverse Kinematic + * @param dest the new position of the joint + * @param currentPose the current pose in which the joint is moved + * @param pickedJoint your picked joint + * @param rotatbleJoints joints, which can be rotate. It has to be a ordered set in this order (picked,marked]. "picked" is our imaginary -1th element, his parent the 0th element and so on. "marked" is the last element. "marked"s matrix can rotate, but the "marked" will not be translated. + */ +void SkeletonEditingPlugin::inverseKinematic(ACG::Vec3d dest,Skeleton::Pose* currentPose,Skeleton::Joint* pickedJoint, std::vector rotatableJoints) +{ + //Now we can rotate every Joint in rotatableJoints + ACG::Vec3d pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); + for(std::size_t i = 5; i && pickedJointPos != dest; --i ) + { + + //iterates through the skeleton from our picked joint to our fixed/root + for (std::vector::iterator iter = rotatableJoints.begin() ; iter != rotatableJoints.end(); ++iter) + { + const unsigned int currentId = (*iter)->id(); + + //when we rotate to the false side + //we are going to compute a new angle + //it is more precise + int angleFac = -1; + bool rightRotation = true; + unsigned int tries = 0;//to be sure that we are terminate + do + { + //update the position of our picked Joint + pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); + + //get the position and the matrix of our current joint + const ACG::Matrix4x4d currentGlobalMatrix = currentPose->globalMatrix(currentId); + const ACG::Vec3d currentJointPos = currentGlobalMatrix.transform_point(ACG::Vec3d(0.0,0.0,0.0)); + + ACG::Vec3d toDest = currentJointPos - dest; //distance from current to Destination + ACG::Vec3d toPickedJoint = currentJointPos - pickedJointPos;//distance from current to picked Joint + + //get angle of toDest and toPickedJoint + double theta = (double)angleFac*acos(dot(toPickedJoint ,toDest) / sqrt( toDest.sqrnorm() * toPickedJoint.sqrnorm())); + + if(theta != theta || theta == 0)//test nan + break; + + //get rotation matrix for theta and our rotation axis + ACG::Vec3d rotationAxis = cross(toDest,toPickedJoint); + ACG::Quaterniond quaternion(rotationAxis ,theta); + + ACG::Matrix4x4d changingMatrix = currentGlobalMatrix * quaternion.rotation_matrix(); + ACG::Vec3d currentTranslation = currentPose->globalTranslation( currentId ); + + //set position and rotation of our currentJoint, so pickedJoint is moving to our destination + currentPose->setGlobalMatrix(currentId, changingMatrix, false); + + //////////////todo compute direction of the angle "theta" + //test new position + //it is false, when the distance of pickedJoint and destination is far more away then before + ACG::Vec3d pickedJointPosOld = pickedJointPos; + pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); + //compare the distance of the old/new position + bool rightRotation = !( (pickedJointPos -dest).sqrnorm() > (pickedJointPosOld -dest).sqrnorm()); + + if ( !rightRotation ) + { + //not the right rotation? then rotate back and compute a new angle + ACG::Quaterniond quaternionBack(rotationAxis ,-theta); + ACG::Vec3d currentTranslation = currentPose->globalTranslation( currentId ); + + //rotate back and compute our angle again + changingMatrix = currentGlobalMatrix * quaternionBack.rotation_matrix(); + + currentPose->setGlobalMatrix(currentId, changingMatrix , false); + currentPose->setGlobalTranslation(currentId, currentTranslation, false); + + //compute angle again and this time, we try the other side for our rotation + ++tries; + angleFac *= 1; + } + }while( rightRotation && tries <= 5); + } + } +} //------------------------------------------------------------------------------ @@ -715,8 +797,9 @@ void SkeletonEditingPlugin::manipulatorMoved( QtTranslationManipulatorNode* _nod if(inverseKinematic_) { Skeleton* skeleton = PluginFunctions::skeleton( object ); + SkeletonObject* skeletonObj = PluginFunctions::skeletonObject(object); - if (skeleton == 0) + if (!skeleton || !skeletonObj) return; Skeleton::Joint* pickedJoint = skeleton->joint( _node->getData().toInt() ); @@ -741,7 +824,6 @@ void SkeletonEditingPlugin::manipulatorMoved( QtTranslationManipulatorNode* _nod //get all Parent Joints (picked,fixed] std::vector rotatableJoints; - Skeleton::Iterator iter = pickedJoint->parent(); //go from our picked Joint and find the first fixed/root Joint @@ -753,84 +835,15 @@ void SkeletonEditingPlugin::manipulatorMoved( QtTranslationManipulatorNode* _nod else return; - //Now we can rotate every Joint in rotatableJoints - ACG::Vec3d pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); - - ACG::Matrix4x4d IMatrix; - IMatrix.identity(); + inverseKinematic(dest, currentPose, pickedJoint, rotatableJoints); - for(std::size_t i = 5; i && pickedJointPos != dest; --i ) - { - - //iterates through the skeleton from our picked joint to our fixed/root + if (transformAllFrames_ && _event->type() == QEvent::MouseButtonRelease) for (std::vector::iterator iter = rotatableJoints.begin() ; iter != rotatableJoints.end(); ++iter) { - const unsigned int currentId = (*iter)->id(); - - //when we rotate to the false side - //we are going to compute a new angle - //it is more precise - int angleFac = -1; - bool rightRotation = true; - unsigned int tries = 0;//to be sure that we are terminate - do - { - //update the position of our picked Joint - pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); - - //get the position and the matrix of our current joint - const ACG::Matrix4x4d currentGlobalMatrix = currentPose->globalMatrix(currentId); - const ACG::Vec3d currentJointPos = currentGlobalMatrix.transform_point(ACG::Vec3d(0.0,0.0,0.0)); - - ACG::Vec3d toDest = currentJointPos - dest; //distance from current to Destination - ACG::Vec3d toPickedJoint = currentJointPos - pickedJointPos;//distance from current to picked Joint - - //get angle of toDest and toPickedJoint - double theta = (double)angleFac*acos(dot(toPickedJoint ,toDest) / sqrt( toDest.sqrnorm() * toPickedJoint.sqrnorm())); - - if(theta != theta || theta == 0)//test nan - break; - - //get rotation matrix for theta and our rotation axis - ACG::Vec3d rotationAxis = cross(toDest,toPickedJoint); - ACG::Quaterniond quaternion(rotationAxis ,theta); - - ACG::Matrix4x4d changingMatrix = currentGlobalMatrix * quaternion.rotation_matrix(); - ACG::Vec3d currentTranslation = currentPose->globalTranslation( currentId ); - - //set position and rotation of our currentJoint, so pickedJoint is moving to our destination - currentPose->setGlobalMatrix(currentId, changingMatrix, false); - currentPose->setGlobalTranslation(currentId, currentTranslation, false); - - //test new position - //it is false, when the distance of pickedJoint and destination is far more away then before - ACG::Vec3d pickedJointPosOld = pickedJointPos; - pickedJointPos = currentPose->globalMatrix(pickedJoint->id()).transform_point(ACG::Vec3d(0.0,0.0,0.0)); - - //compare the distance of the old/new position - bool rightRotation = !( (pickedJointPos -dest).sqrnorm() > (pickedJointPosOld -dest).sqrnorm()); - - if ( !rightRotation ) - { - //not the right rotation? then rotate back and compute a new angle - ACG::Quaterniond quaternionBack(rotationAxis ,-theta); - ACG::Vec3d currentTranslation = currentPose->globalTranslation( currentId ); - - //rotate back and compute our angle again - changingMatrix = currentGlobalMatrix * quaternionBack.rotation_matrix(); - - currentPose->setGlobalMatrix(currentId, changingMatrix , false); - currentPose->setGlobalTranslation(currentId, currentTranslation, false); - - //compute angle again and this time, we try the other side for our rotation - ++tries; - angleFac *= 1; - } - }while( rightRotation && tries <= 5); + ACG::Vec3d position = currentPose->globalTranslation((*iter)->id()); + setJointPosition(skeletonObj,*iter,position); } - } - }//end else picked->isRoot() - + } //update emit updatedObject(objectId, UPDATE_GEOMETRY); } diff --git a/SkeletonEditingPlugin.hh b/SkeletonEditingPlugin.hh index 2594dc183e3f65f1792399948501f288f4b8ce5d..a1ec9af7786e65abf5d6a3570ccf2cb15b345649 100644 --- a/SkeletonEditingPlugin.hh +++ b/SkeletonEditingPlugin.hh @@ -239,6 +239,9 @@ class SkeletonEditingPlugin : public QObject, BaseInterface, MouseInterface, Key /// make sure the manipulator is positioned on a joint void updateManipulatorPosition(BaseObjectData* _skeletonObj); + ///function for computing the position of our joints using inverse Kinematic + void inverseKinematic(ACG::Vec3d dest,Skeleton::Pose* currentPose,Skeleton::Joint* pickedJoint, std::vector rotatableJoints); + private slots: /// move the object when its manipulator moves void manipulatorMoved( QtTranslationManipulatorNode* _node , QMouseEvent* _event);