/**
* 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;
}
