void
SMRBasicRenderer::draw ()
{
GLuint Shader = 0;
// load/get shader info
switch(m_renderStyle)
{
case toon:
Shader = getShader("toonShader");
break;
case gooch:
Shader = getShader("goochShader");
break;
}
bool useShader = isGLSLSupported() && Shader ;
SMRVector3 tmp, tmp2;
glPushAttrib (GL_POLYGON_BIT);
glEnable (GL_CULL_FACE);
// first switch to absolute if needed
m_skeleton->setMode(ABSOLUTEMODE);
glPushMatrix();
glScaled(m_scaleFactor,m_scaleFactor,m_scaleFactor);
if (useShader)
{
if( GLEW_VERSION_2_0 )
glUseProgram( Shader );
else if( GLEW_VERSION_1_5 )
glUseProgramObjectARB( Shader );
glPolygonMode (GL_BACK, GL_FILL);
glCullFace (GL_FRONT);
double size = 0.0001 ;
for(unsigned int i = 0; i < m_skeleton->getNumJoints(); ++i ) {
if( m_skeleton->getJoint(i)->getParentName()!="") {
tmp = m_skeleton->getJoint(i)->getPosition();
tmp2 = m_skeleton->getJointByName(m_skeleton->getJoint(i)->getParentName())->getPosition();
size = drawCylinder(tmp2,tmp,16,size,0.1f);
if (m_skeleton->getJoint(i)->isEndJoint() )
{
SMRVector3 end = m_skeleton->getJoint(i)->getEndLength();
//m_skeleton->getJoint(i)->getOrientation().rotate( end );
//drawCylinder(tmp,tmp+end,16,size,0.0f);
drawCylinder(tmp,end,16,size,0.0f);
}
}
}
if( GLEW_VERSION_2_0 )
glUseProgram( 0 );
else if( GLEW_VERSION_1_5 )
glUseProgramObjectARB( 0 );
/*
* draw back-facing polygons as red lines
*/
/* disable lighting for outlining */
glPushAttrib (GL_LIGHTING_BIT | GL_LINE_BIT | GL_DEPTH_BUFFER_BIT);
glDisable (GL_LIGHTING);
glPolygonMode (GL_FRONT, GL_LINE);
glCullFace (GL_BACK);
glDepthFunc (GL_LEQUAL);
glLineWidth (5.0f);
/* draw wire object */
glColor3f (0.05f, 0.0f, 0.0f);
{double size = 0.0001 ;
for(unsigned int i = 0; i < m_skeleton->getNumJoints(); ++i ) {
if( m_skeleton->getJoint(i)->getParentName()!="") {
tmp = m_skeleton->getJoint(i)->getPosition();
tmp2 = m_skeleton->getJointByName(m_skeleton->getJoint(i)->getParentName())->getPosition();
size = drawCylinder(tmp2,tmp,16,size,0.1f);
glPushMatrix();
glTranslated(tmp2.X(),tmp2.Y(),tmp2.Z());
SMRQuaternion orient = m_skeleton->getJoint(i)->getOrientation();
SMRVector3 axis = orient.getRotationAxis();
double angle = orient.getRotationAngle();
double axisi[3] = {axis.X(),axis.Y(),axis.Z()};
glRotated(angle*180/M_PI,axis.X(),axis.Y(),axis.Z());
//drawAxis(4.0);
glPopMatrix();
if (m_skeleton->getJoint(i)->isEndJoint() )
{
SMRVector3 end = m_skeleton->getJoint(i)->getEndLength();
//m_skeleton->getJoint(i)->getOrientation().rotate( end );
//drawCylinder(tmp,tmp+end,16,size,0.0f);
drawCylinder(tmp,end,16,size,0.0f);
}
}
}
}
/* GL_LIGHTING_BIT | GL_LINE_BIT | GL_DEPTH_BUFFER_BIT */
glPopAttrib ();
/* GL_POLYGON_BIT */
glPopAttrib ();
}
//else
{
//Draw each bone
glBegin( GL_LINES );
for(unsigned int i = 0; i < m_skeleton->getNumJoints(); ++i ) {
if( m_skeleton->getJoint(i)->getParentName()!="") {
tmp = m_skeleton->getJoint(i)->getPosition();
tmp2 = m_skeleton->getJointByName(m_skeleton->getJoint(i)->getParentName())->getPosition();
glVertex3d( tmp2.m_x,tmp2.m_y ,tmp2.m_z);
glVertex3d( tmp.m_x,tmp.m_y,tmp.m_z );
}
}
glEnd();
}
glPopMatrix();
}
void GestureModifier::computeBoundingBox(vector<MotionSegment *> & _motionSegments, SMRVector3 & _lowerBoxBound, SMRVector3 & _higherBoxBound)
{
vector<MotionSegment *>::iterator motionSegIt = _motionSegments.begin();
while ( motionSegIt < _motionSegments.end() && (*motionSegIt)->getGestureModifier() == NULL )
{
motionSegIt++;
}
if ( motionSegIt == _motionSegments.end())
return; //no nucleus assigned to this gesture, just for security reasons, I think this function is not called in that case
//first key pose:
_higherBoxBound = (static_cast<ActuatorHybridIKSlerp*>((*motionSegIt)->getActuator(0)))->getConstraintPtr(0)->getPosition();
_lowerBoxBound = _higherBoxBound;
motionSegIt++;
SMRVector3 currentKeyPoint;
while (motionSegIt != _motionSegments.end() && (*motionSegIt)->getGestureModifier() !=NULL)
{
currentKeyPoint = (static_cast<ActuatorHybridIKSlerp*>((*motionSegIt)->getActuator(0)))->getConstraintPtr(0)->getPosition();
_lowerBoxBound.m_x = min(_lowerBoxBound.X(),currentKeyPoint.X());
_lowerBoxBound.m_y = min(_lowerBoxBound.Y(),currentKeyPoint.Y());
_lowerBoxBound.m_z = min(_lowerBoxBound.Z(),currentKeyPoint.Z());
_higherBoxBound.m_x = max(_higherBoxBound.X(),currentKeyPoint.X());
_higherBoxBound.m_y = max(_higherBoxBound.Y(),currentKeyPoint.Y());
_higherBoxBound.m_z = max(_higherBoxBound.Z(),currentKeyPoint.Z());
motionSegIt++;
}
LOG_INFO(modifierLogger,"BoundingBox is: "<<_lowerBoxBound.X()<<";"<<_lowerBoxBound.Y()<<";"<<_lowerBoxBound.Z()\
<<" - "<<_higherBoxBound.X()<<";"<<_higherBoxBound.Y()<<";"<<_higherBoxBound.Z());
}
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;
}
void GestureModifier::spatialMod(SMRVector3 &origin, const SMRVector3 & _shoulder, const char & _bodyPart){
if (m_v_s != 0.0)
{
LOG_INFO(modifierLogger,"old TARGET: "<<origin.X()<<";"<<origin.Y()<<";"<<origin.Z());
double l = 1.0;
if (_bodyPart == 'r' ) l = -1.0;
double twoPi = 2.0 * M_PI;
double piHalf = M_PI / 2.0;
const double xDiff = _shoulder.X()-origin.X();
const double yDiff = _shoulder.Y()-origin.Y();
const double zDiff = _shoulder.Z()-origin.Z();
const double axDiff = abs(xDiff);
const double ayDiff = abs(yDiff);
const double azDiff = abs(zDiff);
LOG_TRACE(modifierLogger,"bodyPart is :"<<_bodyPart);
LOG_TRACE(modifierLogger,"shoulderPos: "<<_shoulder.X()<<";"<<_shoulder.Y()<<";"<<_shoulder.Z());
LOG_TRACE(modifierLogger,"diff: "<<xDiff<<";"<<yDiff<<";"<<zDiff<<endl);
const double angleDiff = m_v_s * (M_PI /8.0);
//horizontal angle
double alpha = std::atan2(2.5 * ayDiff ,axDiff);
//vertical angle
double beta = std::atan2(2.5 * axDiff,azDiff);
//sagital angle
double gamma = std::atan2(2.5 * azDiff,ayDiff);
LOG_TRACE(modifierLogger,"alpha: "<<alpha<<"beta: "<<beta<<"gamma: "<<gamma<<endl);
if (xDiff<0.0)
{
if (yDiff >0.0)alpha = M_PI - alpha;
else alpha +=M_PI;
}else if (yDiff <0.0) alpha = twoPi -alpha;
if (zDiff<0.0)
{
if (xDiff <0.0)beta = M_PI - beta;
else beta +=M_PI;
}
else if (xDiff >0.0) beta = twoPi -beta;
if (yDiff <0.0)
{
if (zDiff<0.0) gamma =M_PI -gamma;
else gamma += M_PI;
}else if(zDiff>0.0) gamma = twoPi-gamma;
LOG_TRACE(modifierLogger,"corrected alpha: "<<alpha);
LOG_TRACE(modifierLogger,"corrected beta: "<<beta);
LOG_TRACE(modifierLogger,"corrected gamma: "<<gamma);
double alphaRad = (xDiff / (std::cos(alpha)));
double betaRad = (zDiff / (std::cos(beta )));
double gammaRad = (yDiff / (std::cos(gamma)));
LOG_TRACE(modifierLogger,"rads: "<<alphaRad<<" "<<betaRad<<" "<<gammaRad);
//open or close arms
if (m_v_s < 0.0)
{
if (alpha < M_PI)
{
alpha = max((alpha +l * angleDiff) , 0.0);
}else
{
alpha = min((alpha - l* angleDiff) , twoPi);
}
if (beta < M_PI)
{
if (beta < piHalf)
{
beta = min((beta - angleDiff),piHalf);
}else
{
beta = max((beta + angleDiff),piHalf);
}
}else
{
if (beta < (3.0* piHalf))
{
beta = min((beta - angleDiff),3.0* piHalf);
}else
{
beta = max((beta +angleDiff),3.0* piHalf);
}
}
if (gamma < M_PI)
{
if (gamma < piHalf)
{
gamma = max((gamma + 0.3* angleDiff),0.0);
}
else
{
gamma = min((gamma- 0.3*angleDiff),M_PI);
}
}else
{
if (gamma <(3.0*piHalf))
{
gamma = max((gamma + 0.3*angleDiff), M_PI);
}
else
{
gamma = min((gamma - 0.3*angleDiff),twoPi);
}
}
}else
{
if (alpha < M_PI)
{
alpha = min((alpha + l*angleDiff) , M_PI);
}else
{
alpha = max((alpha - l* angleDiff) , M_PI);
}
if (beta < M_PI)
{
if (beta < piHalf)
{
beta = max((beta - angleDiff),0.0);
}else
{
beta = min((beta + angleDiff),M_PI);
}
}else
{
if (beta < (3.0* piHalf))
{
beta = max((beta - angleDiff),M_PI);
}else
{
beta = min((beta + angleDiff),twoPi);
}
}
if (gamma < M_PI)
{
if (gamma < piHalf)
{
gamma = min((gamma + 0.3*angleDiff),piHalf);
}else
{
gamma = max((gamma - 0.3*angleDiff),piHalf);
}
}else
{
if (gamma <(3*piHalf))
{
gamma = min((gamma + 0.3* angleDiff), 3.0*piHalf);
}
else
{
gamma = max((gamma - 0.3*angleDiff),3.0*piHalf);
}
}
}
LOG_TRACE(modifierLogger,"new alpha: "<<alpha<<"new beta: "<<beta<<"new gamma: "<<gamma<<endl);
double hx = xDiff -(alphaRad *(std::cos(alpha)));
double hy = yDiff - alphaRad * (std::sin(alpha)/ 2.5);
double vz = zDiff - (betaRad *(std::cos(beta)));
double vx = xDiff + betaRad *(std::sin(beta)/2.5);
double sy = yDiff - (gammaRad * std::cos(gamma));
double sz = zDiff + gammaRad *(std::sin(gamma)/2.5);
//for the right arm, offsets must be inverted
LOG_TRACE(modifierLogger,"hx: "<<hx<<" hy: "<<hy);
LOG_TRACE(modifierLogger,"vz: "<<vz<<" vx: "<<vx);
LOG_TRACE(modifierLogger,"sy: "<<sy<<" sz: "<<sz<<endl);
origin.m_z += vz + sz;
origin.m_x += hx + vx;
origin.m_y -= sy + hy;
//correct targets to avoid collision with hand or body
if (origin.m_z < 0.65 && (origin.X() * l) <0.2) origin.m_y = min (origin.Y(), -0.15);
if ((origin.X()* l) <0.05 && m_v_s < 0.0) origin.m_x = l * 0.05;
LOG_INFO(modifierLogger,"new TARGET: "<<origin.X()<<";"<<origin.Y()<<";"<<origin.Z()<<endl);
}
}
