Commit 1cd1ab4c authored by Matthias Möller's avatar Matthias Möller

- update: "inverse kinematic" works with "transform whole Animation"

git-svn-id: http://www.openflipper.org/svnrepo/OpenFlipper/branches/Free@13097 383ad7c9-94d9-4d36-a494-682f7c89f535
parent bc7875d5
......@@ -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<Skeleton::Joint*> 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<Skeleton::Joint*>::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<Skeleton::Joint*> 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<Skeleton::Joint*>::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);
}
......
......@@ -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<Skeleton::Joint*> rotatableJoints);
private slots:
/// move the object when its manipulator moves
void manipulatorMoved( QtTranslationManipulatorNode* _node , QMouseEvent* _event);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment