/** * update the Horde tentacle according to Smr _tentacleIK */ static void updateTentacle() { //set of floats storing Horde's tentacle joints state float tx,ty,tz,rx,ry,rz,foo; double rrx,rry,rrz; // a Horde handle towards a tentacle Bone (joint) H3DNode tentacleBone; // and its corresponding node in Smr SMRKinematicJoint* ikJoint; //get the transformation parameters from the SmrTentacle for( unsigned int i=0; i < _tentacleIK->getNumJoints()-1 ; i++) { ikJoint = _tentacleIK->getJoint(i); //get Horde tentacle's corresponding joint joint (Horde) h3dFindNodes(H3DRootNode,ikJoint->getName().c_str(), H3DNodeTypes::Joint); tentacleBone = h3dGetNodeFindResult(0); h3dGetNodeTransform(tentacleBone,&tx,&ty,&tz,&rx,&ry,&rz,&foo,&foo,&foo); //put the rotation values into euler angles (in degrees) ikJoint->getOrientation().toEulerAngles(rrx,rry,rrz); // update tentacle's joint according to Smr kinematic chain equivalent h3dSetNodeTransform( tentacleBone, tx, ty, tz, radToDeg( ((float)(rrx)) ), radToDeg( ((float)(rry)) ), radToDeg( ((float)(rrz)) ), foo, foo, foo ); } // orientate Horde tentacles root joint correctly //No more in the new version ! ikJoint = _tentacleIK->getJoint(0); h3dFindNodes(H3DRootNode,ikJoint->getName().c_str(), H3DNodeTypes::Joint); tentacleBone = h3dGetNodeFindResult(0); h3dGetNodeTransform(tentacleBone,&tx,&ty,&tz,&rx,&ry,&foo,&foo,&foo,&foo); h3dSetNodeTransform(tentacleBone,tx,ty,tz,rx,ry,rz,foo,foo,foo); }
void utils::setEntityPosition(int eID, Vec3f newPos) { //GameEvent translate(GameEvent::E_TRANSLATE_GLOBAL, &newPos, 0); //GameEngine::sendEvent(eID, &translate); unsigned int hiD = GameEngine::entitySceneGraphID(eID); Vec3f p,r,s; h3dGetNodeTransform( (H3DNode)hiD, &p.x,&p.y,&p.z, &r.x,&r.y,&r.z, &s.x,&s.y,&s.z ); h3dSetNodeTransform( (H3DNode)hiD, newPos.x,newPos.y,newPos.z, r.x,r.y,r.z, s.x,s.y,s.z ); };
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; }