void Tutorial02::createKinematicChain() { //currentJoint and previous joint referencing for relevant parenting when creating tentacles joints SMRKinematicJoint *currentJoint,*prevJoint; prevJoint=NULL; //instanciate an empty kinematic chain m_kinematicChain=new SMRKinematicChain(RELATIVEMODE,TRANSLATIONFIRST,"simpleChain"); for(int i=0;i<6;i++) { //instanciate a new SMRKinematicJoint currentJoint= (new SMRKinematicJoint()); string name = "joint"+i; currentJoint->setName(name); //add DOF along X axis currentJoint->addDOF(SMRDOF::XAXIS,-M_PI/2.0f,M_PI/2.0f,SMRUtils::degToRad(0)); //add DOF along Y axis currentJoint->addDOF(SMRDOF::YAXIS,-M_PI/16.0f,M_PI/16.0f,SMRUtils::degToRad(0)); //add DOF along Z axis currentJoint->addDOF(SMRDOF::ZAXIS,-M_PI/2.0f,M_PI/2.0f,SMRUtils::degToRad(0)); //set up joint gain (the more distal,the more gain) currentJoint->getDOF(0)->setGain(0.015*i); currentJoint->getDOF(1)->setGain(0.015*i); currentJoint->getDOF(2)->setGain(0.015*i); //set up joint translation parameters in local frame (bone length) currentJoint->setPosition(0,1.0f,0); //take care of parenting issues if(prevJoint) { currentJoint->setParentName(prevJoint->getName()); } //add the new joint into the inverse kinematics chain m_kinematicChain->insertJoint(currentJoint); //keep a reference of the newly created joint for the next turn (parenting issue) prevJoint=currentJoint; } m_kinematicChain->setStartJointIndex(m_kinematicChain->getJoint(0)->getName()); //force the root joint not to move (tentacle basis should be fix) m_kinematicChain->getJoint(0)->getDOF(0)->setUpperBound(0.0); m_kinematicChain->getJoint(0)->getDOF(0)->setLowerBound(0.0); m_kinematicChain->getJoint(0)->getDOF(1)->setUpperBound(0.0); m_kinematicChain->getJoint(0)->getDOF(1)->setLowerBound(0.0); m_kinematicChain->getJoint(0)->getDOF(2)->setUpperBound(0.0); m_kinematicChain->getJoint(0)->getDOF(2)->setLowerBound(0.0); //add the fly constraint (relative to tentacle's tip) to the tentacle m_ikConstraint=new SMRIKConstraint(SMRVector3(0,0,0),SMRVector3(0,0,0),currentJoint->getName()); }
void Tutorial05::createKinematicChain() { //instantiate an empty kinematic chain m_kinematicChain=new SMRKinematicChain(RELATIVEMODE,TRANSLATIONFIRST,"kinematicChain"); SMRKinematicJoint *currentJoint,*prevJoint; prevJoint=NULL; for(int i=0;i<3;i++) { //instantiate a new SMRKinematicJoint currentJoint= (new SMRKinematicJoint()); //give a name to the joint char name[100]; sprintf(name,"joint%d",i); currentJoint->setName(name); //set up joint translation parameters in local frame (bone length) currentJoint->setPosition(0,2,0); //take care of parenting issues if(prevJoint) { currentJoint->setParentName(prevJoint->getName()); } currentJoint->addDOF( SMRDOF::YAXIS, 0, 0, 0, 0 ); currentJoint->addDOF( SMRDOF::XAXIS, -M_PI/1.0, +M_PI/1.0, 0, 0 ); currentJoint->addDOF( SMRDOF::YAXIS, 0, 0, 0, 0 ); currentJoint->addDOF( SMRDOF::ZAXIS, -M_PI/1.0, +M_PI/1.0, 0, 0 ); currentJoint->addDOF( SMRDOF::YAXIS, -M_PI/1.0, +M_PI/1.0, 0, 0 ); //add the new joint into the kinematics chain m_kinematicChain->insertJoint(currentJoint); //keep a reference of the newly created joint for the next turn (parenting issue) prevJoint=currentJoint; } //SMRQuaternion rot; //rot.fromEulerAngles(M_PI/2,0.0,0.0); //m_kinematicChain->getJoint(1)->setOrientation(rot); //m_kinematicChain->getJoint(2)->setOrientation(rot); //set root joint m_kinematicChain->setStartJointIndex(m_kinematicChain->getJoint(1)->getName()); //add the fly constraint (relative to tentacle's tip) to the tentacle m_ikConstraint=new SMRIKConstraint(SMRVector3(0,0,0),SMRVector3(0,0,0),currentJoint->getName()); }
void Tutorial01::updateKinematicChain() { SMRKinematicJoint *joint; //rotate first joint joint=m_kinematicChain->getJoint(0); joint->setOrientation(SMRQuaternion(SMRVector3(1,0,0),sinf(m_time))); //rotate second joint joint=m_kinematicChain->getJoint(1); joint->setOrientation(SMRQuaternion(SMRVector3(1,0,0),sinf(m_time))); //rotate third joint joint=m_kinematicChain->getJoint(2); joint->setOrientation(SMRQuaternion(SMRVector3(1,0,0),sinf(m_time))); }
void Tutorial02::onUpdate() { float tx,ty,tz; m_flyNode.getPosition(&tx,&ty,&tz); //roughly simulates the random fly of a fly if((rand()%100)>95) { m_dx=((SMRUtils::ranf()))/50.0f; m_dy=((SMRUtils::ranf()))/50.0f; m_dz=((SMRUtils::ranf()))/50.0f; } //change fly position m_flyNode.setPosition(tx+m_dx,ty+m_dy,tz+m_dz); if(tx<-2.0) m_dx=0.05f; if(tx>2.0) m_dx=-0.05f; if(ty<2.0) m_dy=0.05f; if(ty>4.0) m_dy=-0.05f; if(tz<-2.0) m_dz=0.05f; if(tz>2.0) m_dz=-0.05f; m_flyNode.getPosition(&tx,&ty,&tz); //update constraint position according to fly position m_ikConstraint->setPosition(SMRVector3(tx,ty,tz)); //compute inverse kinematics m_ikSolver->process(); //update visual representations m_bones.update(); }
SMRDOF::SMRDOF() { m_quaternionRotation.identity(); m_radRotation = 0.0f; m_axis = XAXIS; m_rotationAxis = SMRVector3(1.0,0.0,0.0); m_lowerBound = -2*M_PI; m_upperBound = 2*M_PI; m_gain = 5.0f; }
void GestureModifier::spatialExtent(SMRVector3 &origin) { if ( m_v_s != 0.0f) { const SMRVector3 cOGS = SMRVector3(0.0f, -0.22f, 0.28f); LOG_DEBUG(modifierLogger,origin.m_x<<";"<<origin.m_y<<";"<<origin.m_z<<"is the old target"); float scaleFactor = 1 + (m_v_s * 0.35f); origin = cOGS + ((origin - cOGS).operator *(scaleFactor)); LOG_DEBUG(modifierLogger,origin.m_x<<";"<<origin.m_y<<";"<<origin.m_z<<"is the NEW target"); } }
void Tutorial03::createKinematicChain() { float tx,ty,tz,rx,ry,rz; //currentJoint and previous joint referencing for relevant parenting when creating tentacles joints SMRKinematicJoint *currentJoint,*prevJoint; prevJoint=NULL; //instanciate an empty kinematic chain m_kinematicChain=new SMRKinematicChain(RELATIVEMODE,TRANSLATIONFIRST,"tentacle"); //get Hordes m_kinematicChain first joint HordeNode firstNode= m_tentacleNode.getFirstChild(); //will help computing Smr joints gain int i=0; //while Horde tentacles joints are found while(firstNode.isValid()) { //instanciate a new SMRKinematicJoint currentJoint= (new SMRKinematicJoint()); //get joint translation parameters in parents local frame (bone length) firstNode.getTransform(&tx,&ty,&tz,&rx,&ry,&rz); //add DOF along X axis currentJoint->addDOF(SMRDOF::XAXIS,-M_PI/16.0f,M_PI/16.0f,SMRUtils::degToRad(rx)); //add DOF along Y axis currentJoint->addDOF(SMRDOF::YAXIS,-M_PI/4.0f,M_PI/4.0f,SMRUtils::degToRad(ry)); //add DOF along Z axis currentJoint->addDOF(SMRDOF::ZAXIS,-M_PI/4.0f,M_PI/4.0f,SMRUtils::degToRad(rz)); //set up joint gain (the more distal,the more gain) currentJoint->getDOF(0)->setGain(0.01*i); currentJoint->getDOF(1)->setGain(0.01*i); currentJoint->getDOF(2)->setGain(0.01*i); //give a name to the joint currentJoint->setName(firstNode.getName()); //set up joint translation parameters in local frame (bone length) currentJoint->setPosition(tx,ty,tz); //take care of parenting issues if(prevJoint) { currentJoint->setParentName(prevJoint->getName()); } //and add the brand new joint into the inverse kinematics chain ! m_kinematicChain->insertJoint(currentJoint); //keep a reference of the newly created joint for the next turn (parenting issue) prevJoint=currentJoint; //get the next joint according to Horde structure firstNode=firstNode.getFirstChild(); //increase the gain increment i++; } //m_kinematicChain->setStartJointIndex(m_kinematicChain->getJoint(0)->getName()); m_kinematicChain->setStartJointIndexInt(0); //force the root joint not to move (tentacle basis should be fix) m_kinematicChain->getJoint(0)->getDOF(0)->setUpperBound(0); m_kinematicChain->getJoint(0)->getDOF(0)->setLowerBound(0); m_kinematicChain->getJoint(0)->getDOF(1)->setUpperBound(0); m_kinematicChain->getJoint(0)->getDOF(1)->setLowerBound(0); m_kinematicChain->getJoint(0)->getDOF(2)->setUpperBound(0); m_kinematicChain->getJoint(0)->getDOF(2)->setLowerBound(0); //and add the fly constraint (relative to tentacle's tip) to the tentacle m_ikConstraint=new SMRIKConstraint(SMRVector3(0,0,0),SMRVector3(0,0,0),currentJoint->getName()); }
void TentacleApplication::mainLoop( float fps ) { _curFPS = fps; _timer += 1 / fps; keyHandler(); h3dSetOption( H3DOptions::DebugViewMode, _debugViewMode ? 1.0f : 0.0f ); h3dSetOption( H3DOptions::WireframeMode, _wireframeMode ? 1.0f : 0.0f ); if( !_freeze ) { float tx,ty,tz,foo; h3dGetNodeTransform(_fly,&tx,&ty,&tz,&foo,&foo,&foo,&foo,&foo,&foo); if (!_freezeFly) { // routhly simulates the random fly of a fly if((rand() % 100)>95) { _dx = ((ranf()))/50.0f; _dy = ((ranf()))/50.0f; _dz = ((ranf()))/50.0f; } // change fly position h3dSetNodeTransform( _fly, tx+_dx, ty+_dy, tz+_dz, 0, 0, 0, 0.2f, 0.2f, 0.2f ); if (tx < -4.0) _dx = 0.05f; if (tx > 4.0) _dx = -0.05f; if (ty < 0.0) _dy = 0.05f; if (ty > 6.0) _dy = -0.05f; if (tz < -4.0) _dz = 0.05f; if (tz > 4.0) _dz = -0.05f; h3dGetNodeTransform(_fly,&tx,&ty,&tz,&foo,&foo,&foo,&foo,&foo,&foo); //update constraint position according to fly position _ikConstraint->setPosition(SMRVector3(tx, ty, tz)); } if (!_freezeIK || _ikStep) { //Compute inverse kinematics _currentSolver->process(); // update tentacle's joint (IK) updateTentacle(); _ikStep = false; } } // Set camera parameters h3dSetNodeTransform( _cam, _x, _y, _z, _rx ,_ry, 0, 1, 1, 1 ); if( _showFPS ) { // Avoid updating FPS text every frame to make it readable if( _timer > 0.3f ) { _fpsText.str( "" ); _fpsText << "FPS: " << fixed << setprecision( 2 ) << _curFPS; _timer = 0; } // Show text //h3dutShowText( _fpsText.str().c_str(), 0, 0.95f, 0.03f, 0, _fontMatRes ); } // Show title //h3dutShowText( "Comparing Standard IK algorithms.", 0.0f, 0.90f, 0.03f, 0, _fontMatRes ); // Show IK method //h3dutShowText( _ikMethodString, 0.6f, 0.80f, 0.03f, 0, _fontMatRes ); // Show logo //h3dShowOverlay( 0.7f, 0.2, 0, 0, // 1, 0.2, 1, 0, // 1, 0.6f, 1, 1, // 0.7f, 0.6f, 0, 1, // 7, _invJacMatRes ); // Render scene h3dRender( _cam ); // Remove all overlays h3dClearOverlays(); // Write all mesages to log file h3dutDumpMessages(); }
/** * set up Smr IK tentacke according to Horde tentacle morphology */ SMRKinematicChain * setUpKinematikChain(H3DNode _firstNode) { float tx,ty,tz,rx,ry,rz,foo; // currentJoint and previous joint referencing for relevant parenting when creating tentacles joints SMRKinematicJoint *currentJoint, *prevJoint; currentJoint = NULL; prevJoint = NULL; //prevJoint = NULL; // instanciate an empty kinematic chain SMRKinematicChain * tentacle = new SMRKinematicChain(RELATIVEMODE,TRANSLATIONFIRST,"tentacle"); // get Hordes tentacle first joint _firstNode = h3dGetNodeChild(_firstNode,0); // again //_firstNode = h3dGetNodeChild(_firstNode,0); // will help computing Smr joints gain int i = 0; // while Horde tentacles joints are found while (_firstNode >0) { //instanciate a new SMRKinematicJoint currentJoint = (new SMRKinematicJoint()); //get joint translation parameters in parents local frame (bone length) h3dGetNodeTransform(_firstNode,&tx,&ty,&tz,&rx,&ry,&rz,&foo,&foo,&foo); //add DOF along X axis currentJoint->addDOF(SMRDOF::XAXIS,-M_PI/16.0f,M_PI/16.0f,degToRad(rx)); //add DOF along Y axis currentJoint->addDOF(SMRDOF::YAXIS,-M_PI/3.0f,M_PI/3.0f,degToRad(ry)); //add DOF along Z axis currentJoint->addDOF(SMRDOF::ZAXIS,-M_PI/3.0f,M_PI/3.0f,degToRad(rz)); //set up joint gain (the more distal, the more gain) currentJoint->getDOF(0)->setGain(0.015*i); currentJoint->getDOF(1)->setGain(0.015*i); currentJoint->getDOF(2)->setGain(0.015*i); //give a name to the joint (same as Horde) currentJoint->setName(h3dGetNodeParamStr(_firstNode, H3DNodeParams::NameStr)); //set up joint translation parameters in local frame (bone length) currentJoint->setPosition(tx,ty,tz); // take care of parenting issues if (prevJoint) { currentJoint->setParentName(prevJoint->getName()); } // and add the brand new joint into the inverse kinematics chain ! tentacle->insertJoint(currentJoint); // keep a reference of the newly created joint for the next turn (parenting issue) prevJoint = currentJoint; // get the next joint according to Horde structure _firstNode = h3dGetNodeChild(_firstNode,0); // increase the gain increment i++; } tentacle->setStartJointIndex(tentacle->getJoint(0)->getName()); // force the root joint not to move (tentacle basis should be fix) tentacle->getJoint(0)->getDOF(0)->setUpperBound(0); tentacle->getJoint(0)->getDOF(0)->setLowerBound(0); tentacle->getJoint(0)->getDOF(1)->setUpperBound(0); tentacle->getJoint(0)->getDOF(1)->setLowerBound(0); tentacle->getJoint(0)->getDOF(2)->setUpperBound(0); tentacle->getJoint(0)->getDOF(2)->setLowerBound(0); // and add the fly constraint (relative to tentacles' tip) to the tentacle _ikConstraint = new SMRIKConstraint(SMRVector3(0,0,0),SMRVector3(0,0,0),currentJoint->getName()); // tentacle is instanciated and set up ! return tentacle; }
double drawCylinder(SMRVector3 _begin, SMRVector3 _end, unsigned int _step, double _sizeBeg, double _sizeEnd) { // angular displacement double stepRad = 2*M_PI/_step; // temporary points SMRVector3 tmp1, tmp2,tmp3,tmp4; SMRVector3 noise(0.0001f,0.0001f,0.0001f); SMRVector3 unit = _end - _begin + noise; double length = unit.norm(); unit.normalize(); SMRVector3 ortho = SMRVector3(-unit.m_y, unit.m_x, 0); ortho.normalize(); // draw begin cap //tmp1 = ortho * length * _sizeBeg; tmp1 = ortho * _sizeBeg; glBegin(GL_TRIANGLE_FAN); glNormal3d(-unit.m_x,-unit.m_y,-unit.m_z); glVertex3d(_begin.m_x,_begin.m_y,_begin.m_z); for (unsigned int i = 0 ; i <=_step ; i++){ tmp2 = tmp1; SMRQuaternion quat(unit,double(i*stepRad)); //quat = RotationQuaternion(unit,double(i*stepDeg) ); quat.rotate(tmp2); tmp2 = tmp2 + _begin; glVertex3d(tmp2.m_x,tmp2.m_y,tmp2.m_z); } glEnd(); // draw cylinder //tmp1 = ortho * length * _sizeBeg; tmp1 = ortho * _sizeBeg; tmp3 = ortho * length * _sizeEnd; glBegin(GL_TRIANGLE_STRIP); glNormal3d(ortho.m_x,ortho.m_y,ortho.m_z); glVertex3d(_begin.m_x + tmp1.m_x,_begin.m_y + tmp1.m_y,_begin.m_z + tmp1.m_z); glVertex3d(_end.m_x + tmp3.m_x,_end.m_y + tmp3.m_y,_end.m_z + tmp3.m_z); for (unsigned int i = 0 ; i <=_step ; i++){ tmp2 = tmp1; tmp4 = tmp3; SMRQuaternion quat(unit,double(i*stepRad)); //quat = RotationQuaternion(unit,double(i*stepDeg) ); quat.rotate(tmp2); quat.rotate(tmp4); SMRVector3 tmp5 = tmp2;tmp5.normalize(); glNormal3d(tmp5.m_x,tmp5.m_y,tmp5.m_z); glVertex3d(_begin.m_x + tmp2.m_x,_begin.m_y + tmp2.m_y,_begin.m_z + tmp2.m_z); glVertex3d(_end.m_x + tmp4.m_x,_end.m_y + tmp4.m_y,_end.m_z + tmp4.m_z); } glEnd(); // draw end cap tmp1 = ortho * length * _sizeEnd; glBegin(GL_TRIANGLE_FAN); glNormal3d(unit.m_x,unit.m_y,unit.m_z); glVertex3d(_end.m_x,_end.m_y,_end.m_z); for (unsigned int i = 0 ; i <=_step ; i++){ tmp2 = tmp1; SMRQuaternion quat(unit,double(i*stepRad)); //quat = RotationQuaternion(unit,double(i*stepDeg) ); quat.rotate(tmp2); tmp2 = tmp2 + _end; glVertex3d(tmp2.m_x,tmp2.m_y,tmp2.m_z); } glEnd(); return (length * _sizeEnd)+0.0001; }