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()); }
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::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); } }