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