// Overwrite base class update //---------------------------------------------------------------------------------------------------------------------- void ArmMuscledViz::update() { // Let simple viz do its rendering first //ArmViz::update(); glPushAttrib(GL_LIGHTING); glDisable(GL_LIGHTING); glDisable(GL_DEPTH_TEST); // Get shoulder and elbow TMs and other useful data const float shdAngle = m_arm->getJointAngle(JT_shoulder); const float elbAngle = m_arm->getJointAngle(JT_elbow); const ci::Vec3f elbPos = Vec3f(m_arm->getElbowPos()); //const ci::Vec3f effPos = Vec3f(m_arm->getEffectorPos()); const ci::Vec3f shdPos = Vec3f(0,0,0); const double elbRad = m_arm->getJointRadius(JT_elbow); const double shdRad = m_arm->getJointRadius(JT_shoulder); ci::Matrix44f elbLimTM; elbLimTM.setToIdentity(); elbLimTM.rotate(Vec3f(0.0f, 0.0f, 1.0f), shdAngle); elbLimTM.setTranslate(elbPos); ci::Matrix44f elbTM = elbLimTM; elbTM.rotate(Vec3f(0,0,1), elbAngle); ci::Matrix44f shdTM; shdTM.setToIdentity(); shdTM.rotate(Vec3f(0.0f, 0.0f, 1.0f), shdAngle); shdTM.setTranslate(shdPos); // Overall position and orientation glPushMatrix(); glMultMatrixf(*m_pTM); glLineWidth(1.0); glColor3f(0,0,0); // Desired kinematic state if(m_drawDesiredState) { Pos p1, p2; double desElbAngle = m_arm->getDesiredJointAngle(JT_elbow); double desShdAngle = m_arm->getDesiredJointAngle(JT_shoulder); m_arm->forwardKinematics(desElbAngle, desShdAngle, p1, p2); Vec3f desShdPos(0,0,0); Vec3f desElbPos(p1); Vec3f desEffPos(p2); ci::gl::color(m_colors.desired); // Draw bones glEnable(GL_LINE_STIPPLE); glLineStipple(2, 0xAAAA); ci::gl::drawLine(desShdPos, desElbPos); ci::gl::drawLine(desElbPos, desEffPos); glDisable(GL_LINE_STIPPLE); // Points indicating joints glPointSize(4.0); glBegin(GL_POINTS); glVertex3f(desShdPos.x, desShdPos.y, 0.0); glVertex3f(desElbPos.x, desElbPos.y, 0.0); glVertex3f(desEffPos.x, desEffPos.y, 0.0); glEnd(); } // Draw elbow joint and bone glPushMatrix(); glMultMatrixf(elbTM); // bone triangle ci::gl::color(m_colors.boneFill); ci::Vec3f lt = -elbRad * ci::Vec3f(0,1,0); ci::Vec3f rt = elbRad * ci::Vec3f(0,1,0); ci::Vec3f bt = m_arm->getLength(JT_elbow) * ci::Vec3f(1,0,0); drawTriangle(rt, lt, bt); ci::gl::color(m_colors.boneOutline); drawTriangle(rt, lt, bt, GL_LINE); ci::gl::drawLine(ci::Vec2f(&bt.x), elbRad * ci::Vec2f(1,0)); // joint disk ci::gl::color(m_colors.jointFill); ci::gl::drawSolidCircle(ci::Vec2f(0,0), elbRad, 32); ci::gl::color(m_colors.jointOutline); ci::gl::drawStrokedCircle(ci::Vec2f(0,0), elbRad, 32); glPopMatrix(); // Draw elbow limits glPushMatrix(); glMultMatrixf(elbLimTM); float limMin = radiansToDegrees(m_arm->getJointLimitLower(JT_elbow)); float limMax = radiansToDegrees(m_arm->getJointLimitUpper(JT_elbow)); ci::gl::color(m_colors.limitsFill); drawPartialDisk(elbRad, elbRad + 0.01, 16, 1, 90 - limMin, -(limMax - limMin)); glPopMatrix(); // Draw shoulder joint and bone glColor3f(0,0,0); glPushMatrix(); glMultMatrixf(shdTM); // bone triangle lt = -shdRad * ci::Vec3f(0,1,0); rt = shdRad * ci::Vec3f(0,1,0); bt = m_arm->getLength(JT_shoulder) * ci::Vec3f(1,0,0); ci::gl::color(m_colors.boneFill); drawTriangle(rt, lt, bt); ci::gl::color(m_colors.boneOutline); drawTriangle(rt, lt, bt, GL_LINE); ci::gl::drawLine(m_arm->getLength(JT_shoulder) * ci::Vec2f(1,0), shdRad * ci::Vec2f(1,0)); // joint disk ci::gl::color(m_colors.jointFill); ci::gl::drawSolidCircle(ci::Vec2f(0,0), shdRad, 32); ci::gl::color(m_colors.jointOutline); ci::gl::drawStrokedCircle(ci::Vec2f(0,0), shdRad, 32); glPopMatrix(); // Draw shoulder limits limMin = m_arm->getJointLimitLower(JT_shoulder) * RAD_TO_DEG; limMax = m_arm->getJointLimitUpper(JT_shoulder) * RAD_TO_DEG; ci::gl::color(m_colors.limitsFill); drawPartialDisk(shdRad, shdRad + 0.01, 16, 1, 90 - limMin, -(limMax - limMin)); glLineWidth(1.0); // Trajectory if(m_drawOverlays) { glEnableClientState(GL_VERTEX_ARRAY); glEnableClientState(GL_COLOR_ARRAY); const std::deque<Pos>& effTrajectory = m_arm->getTrajectory(); int numPoints = effTrajectory.size(); float lineVerts[numPoints*2]; float colors[numPoints*4]; glVertexPointer(2, GL_FLOAT, 0, lineVerts); // 2d positions glColorPointer(4, GL_FLOAT, 0, colors); // 4d colors for(size_t i = 0; i < numPoints; i++) { lineVerts[i*2 + 0] = effTrajectory[i].x; lineVerts[i*2 + 1] = effTrajectory[i].y; float a = 0.5 * (float)i / (float)numPoints; colors[i*4 + 0] = m_colors.trajectory[0]; colors[i*4 + 1] = m_colors.trajectory[1]; colors[i*4 + 2] = m_colors.trajectory[2]; colors[i*4 + 3] = a; } glLineWidth(2.0); glDrawArrays( GL_LINE_STRIP, 0, numPoints); glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_COLOR_ARRAY); glLineWidth(1.0); } // Draw muscles for(size_t i = 0; i < m_arm->getNumMuscles(); ++i) { Muscle* muscle = m_arm->getMuscle(i); double act = muscle->getActivation(); // Muscle line width dependent on activation float lineWidth = act * 32.0; glLineWidth(lineWidth); // Muscle colour dependent on length float l = muscle->getNormalisedLength() - 1; l = clamp(l, -1.0f, 1.0f); ci::ColorA col = m_colors.midMuscle; if(l >= 0) { col += (m_colors.longMuscle - m_colors.midMuscle) * l; } else { col -= (m_colors.shortMuscle - m_colors.midMuscle) * l; } ci::gl::color(col); // Draw origin and insertion points Vec3f origin = Vec3f(muscle->getOriginWorld()); Vec3f insertion = Vec3f(muscle->getInsertionWorld()); glPointSize(4.0); glBegin(GL_POINTS); glVertex3f(origin.x, origin.y, 0); glVertex3f(insertion.x, insertion.y, 0); glEnd(); if(muscle->isMonoArticulate()) { MuscleMonoWrap* m = ((MuscleMonoWrap*)muscle); if(!m->m_muscleWraps) { //ci::Vec2f dir = m->getInsertionWorld() - m->getOriginWorld(); //dir.normalize(); // Only hold in the non-wrapping case. Otherwise we need to do a proper projection (dot product). //ci::Vec2f closestPoint = m->getOriginWorld() + dir * m->m_originCapsuleDist; //ci::Vec2f maVec = closestPoint - m_arm->getElbowPos(); //float ma = maVec.length(); //const bool muscleWraps = ma < r && m_arm->getJointAngle(JT_elbow) < PI_OVER_TWO; //ci::gl::drawLine(ci::Vec3f(closestPoint), ci::Vec3f(m_arm->getElbowPos())); ci::gl::drawLine(origin, insertion); // Indicate optimal length //ci::Vec3f l0Pos = origin + m->getOptimalLength() * (insertion - origin).normalized(); //drawPoint(l0Pos, 4.0); } else { // Upstream segment ci::Vec2f pathDir = (m->m_joint == JT_elbow) ? m_arm->getElbowPos() : ci::Vec2f(1, 0); float rotDir = m->m_isFlexor ? 1.0 : -1.0; pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle)); pathDir.normalize(); ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist); ci::gl::drawLine(origin, ci::Vec3f(pathEnd)); // Downstream segment pathDir = (m->m_joint == JT_elbow) ? (m_arm->getElbowPos() - m_arm->getEffectorPos()) : ( -m_arm->getElbowPos()); pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle)); pathDir.normalize(); pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist); ci::gl::drawLine(insertion, ci::Vec3f(pathEnd)); } } // is mono else { glPushAttrib (GL_LINE_BIT); glEnable(GL_LINE_STIPPLE); glLineStipple (1, 0xAAAA); MuscleBiWrap* m = ((MuscleBiWrap*)muscle); if (m->wrapsElbow() && m->wrapsShoulder()) { //glColor3f(0,0,1); // Upstream segment: always shoulder ci::Vec2f pathDir = ci::Vec2f(1, 0); float rotDir = m->m_isFlexor ? 1.0 : -1.0; pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle)); pathDir.normalize(); ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist); ci::gl::drawLine(origin, ci::Vec3f(pathEnd)); // Downstream segment: always elbow. pathDir = m_arm->getElbowPos() - m_arm->getEffectorPos(); pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle)); pathDir.normalize(); pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist); ci::gl::drawLine(insertion, ci::Vec3f(pathEnd)); // Middle segment pathDir = m_arm->getElbowPos(); pathDir.rotate(rotDir * m->m_gammaAngle); pathDir.normalize(); pathDir = pathDir * m->m_momentArms[JT_shoulder]; ci::Vec2f pathStart = pathDir; pathDir = m_arm->getEffectorPos() - m_arm->getElbowPos(); pathDir.rotate(rotDir * (m->m_insertCapsuleAngle + m->getWrapAngle(JT_elbow))); pathDir.normalize(); pathDir = pathDir * m_arm->getJointRadius(JT_elbow); pathEnd = m_arm->getElbowPos() + pathDir; ci::gl::drawLine(ci::Vec3f(pathStart), ci::Vec3f(pathEnd)); } else if (m->wrapsElbow()) { //glColor3f(0,1,0); // Downstream segment: always elbow. Upstream segment from origin to capsule ci::Vec2f pathDir = ci::Vec2f(m_arm->getElbowPos() - m_arm->getEffectorPos()); float rotDir = m->m_isFlexor ? 1.0 : -1.0; pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle)); pathDir.normalize(); ci::Vec2f pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist); ci::gl::drawLine(insertion, ci::Vec3f(pathEnd)); pathDir = m_arm->getEffectorPos() - m_arm->getElbowPos(); pathDir.rotate(rotDir * (m->m_insertCapsuleAngle + m->getWrapAngle(JT_elbow))); pathDir.normalize(); pathDir = pathDir * m_arm->getJointRadius(JT_elbow); pathEnd = m_arm->getElbowPos() + pathDir; ci::gl::drawLine(origin, ci::Vec3f(pathEnd)); } else if (m->wrapsShoulder()) { //glColor3f(1,0,0); // Upstream segment: always shoulder ci::Vec2f pathDir = ci::Vec2f(1, 0); float rotDir = m->m_isFlexor ? 1.0 : -1.0; pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle)); pathDir.normalize(); ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist); ci::gl::drawLine(origin, ci::Vec3f(pathEnd)); //Downstream segment: from capsule to insertion pathDir = m_arm->getElbowPos(); pathDir.rotate(rotDir * m->m_gammaAngle); pathDir.normalize(); pathDir = pathDir * m->m_momentArms[JT_shoulder]; ci::Vec2f pathStart = pathDir; ci::gl::drawLine(ci::Vec3f(pathStart), insertion); } else { // No wrap //glColor3f(1,1,1); ci::gl::drawLine(origin, insertion); } glPopAttrib(); } } // for all muscles glLineWidth(1.0); glPopMatrix(); glEnable(GL_DEPTH_TEST); glPopAttrib(); }