virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override { const Vec3 frcColors[] = {Red,Orange,Cyan}; const Vec3 momColors[] = {Blue,Green,Purple}; m_system.realize(state, Stage::Velocity); const int ncont = m_compliant.getNumContactForces(state); for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); const Vec3& frc = force.getForceOnSurface2()[1]; const Vec3& mom = force.getForceOnSurface2()[0]; Real frcMag = frc.norm(), momMag=mom.norm(); int frcThickness = 1, momThickness = 1; Real frcScale = ForceScale, momScale = ForceScale; while (frcMag > 10) frcThickness++, frcScale /= 10, frcMag /= 10; while (momMag > 10) momThickness++, momScale /= 10, momMag /= 10; DecorativeLine frcLine(force.getContactPoint(), force.getContactPoint() + frcScale*frc); DecorativeLine momLine(force.getContactPoint(), force.getContactPoint() + momScale*mom); frcLine.setColor(frcColors[id%3]); momLine.setColor(momColors[id%3]); frcLine.setLineThickness(2*frcThickness); momLine.setLineThickness(2*momThickness); geometry.push_back(frcLine); geometry.push_back(momLine); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int i=0; i < patch.getNumDetails(); ++i) { const ContactDetail& detail = patch.getContactDetail(i); const Real peakPressure = detail.getPeakPressure(); // Make a black line from the element's contact point in the normal // direction, with length proportional to log(peak pressure) // on that element. DecorativeLine normal(detail.getContactPoint(), detail.getContactPoint()+ std::log10(peakPressure) * detail.getContactNormal()); normal.setColor(Black); geometry.push_back(normal); // Make a red line that extends from the contact // point in the direction of the slip velocity, of length 3*slipvel. DecorativeLine slip(detail.getContactPoint(), detail.getContactPoint()+3*detail.getSlipVelocity()); slip.setColor(Red); geometry.push_back(slip); } } }
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override { const Vec3 frcColors[] = {Red,Orange,Cyan,Green,Blue,Yellow}; m_system.realize(state, Stage::Velocity); const int ncont = m_compliant.getNumContactForces(state); for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); // define if it's femur_lat -> femur_med contact pair: id=8 //if (id == 6 || id == 8 || id==4 || id==2 || id==10 ) // continue; ContactSnapshot cs = m_compliant.getContactTrackerSubsystem().getActiveContacts(state); //cout << "contact: " << id << endl; //ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(2), ContactSurfaceIndex(3)); //ContactId idMeniscFemur1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2)); //ContactId idMeniscTibia1 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(4)); //ContactId idMeniscFemur2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(3)); //ContactId idMeniscTibia2 = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(4)); ContactId idLatFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(2)); ContactId idMedFemurTibia = cs.getContactIdForSurfacePair( ContactSurfaceIndex(1), ContactSurfaceIndex(2)); ContactId idFemur = cs.getContactIdForSurfacePair( ContactSurfaceIndex(0), ContactSurfaceIndex(1)); if (id == idFemur) continue; const SimbodyMatterSubsystem& matter = m_compliant.getMultibodySystem().getMatterSubsystem(); // get tibia's mobilized body MobilizedBody tibiaMobod = matter.getMobilizedBody(MobilizedBodyIndex(22)); MobilizedBody femurLatmobod = matter.getMobilizedBody(MobilizedBodyIndex(19)); MobilizedBody femurMedmobod = matter.getMobilizedBody(MobilizedBodyIndex(20)); //for (int numContacts=0; numContacts<cs.getNumContacts(); numContacts++) //{ //ContactSurfaceIndex csi1 = cs.getContact(numContacts).getSurface1(); //ContactSurfaceIndex csi2 = cs.getContact(numContacts).getSurface2(); //cout << "surf1: " << csi1 << " surf2: " << csi2 << endl; //} if (cs.getNumContacts()>0) { DecorativeGeometry decTibiaContactGeometry = tibiaMobod.getBody().updDecoration(0); decTibiaContactGeometry.setOpacity( 0.9); decTibiaContactGeometry.setColor( Gray); DecorativeGeometry decFemurLatContactGeometry = femurLatmobod.getBody().updDecoration(0); decFemurLatContactGeometry.setOpacity(1); decFemurLatContactGeometry.setColor(Gray); DecorativeGeometry decFemurMedContactGeometry = femurMedmobod.getBody().updDecoration(0); decFemurMedContactGeometry.setOpacity(1); decFemurMedContactGeometry.setColor(Gray); // Tibia transformation const Transform& X_BM = tibiaMobod.getOutboardFrame(state); // M frame in B const Transform& X_GB = tibiaMobod.getBodyTransform(state); // B in Ground const Transform& X_PF = tibiaMobod.getInboardFrame(state); Transform X_GM = X_GB*X_BM; // M frame in Ground // rotate //Rotation& newRot = X_GM.updR(); //Rotation parentRot = X_PF.R(); //newRot.operator*=( parentRot.invert()); // translate in front of the whole body X_GM.setP( X_GM.operator+=( Vec3(0.3,0,0)).p()); // set new transform decTibiaContactGeometry.setTransform( X_GM); // Medial Femur Transformation const Transform& X_BM_medFemur = femurMedmobod.getOutboardFrame(state); // M frame in B const Transform& X_GB_medFemur = femurMedmobod.getBodyTransform(state); // B in Ground const Transform& X_PF_medFemur = femurMedmobod.getInboardFrame(state); Transform X_GM_medFemur = X_GB_medFemur*X_BM_medFemur; // M frame in Ground // rotate //Rotation& newRot_medFemur = X_GM_medFemur.updR(); //Rotation parentRot_medFemur = X_PF_medFemur.R(); //newRot_medFemur.operator*=(parentRot_medFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis())); //newRot_medFemur.setRotationFromAngleAboutZ(1.570796326794897); // translate in front of the whole body X_GM_medFemur.setP(X_GM_medFemur.operator+=(Vec3(0.3, 0.1, 0)).p()); // set new transform decFemurMedContactGeometry.setTransform(X_GM_medFemur); // Lateral Femur Transformation const Transform& X_BM_latFemur = femurLatmobod.getOutboardFrame(state); // M frame in B const Transform& X_GB_latFemur = femurLatmobod.getBodyTransform(state); // B in Ground const Transform& X_PF_latFemur = femurLatmobod.getInboardFrame(state); Transform X_GM_latFemur = X_GB_latFemur*X_BM_latFemur; // M frame in Ground // rotate //Rotation& newRot_latFemur = X_GM_latFemur.updR(); //Rotation parentRot_latFemur = X_PF_latFemur.R(); //newRot_latFemur.operator*=(parentRot_latFemur).operator*=(Rotation(1.57079, CoordinateAxis::ZCoordinateAxis())); //newRot_latFemur.setRotationFromAngleAboutZ(1.570796326794897); // translate in front of the whole body X_GM_latFemur.setP(X_GM_latFemur.operator+=(Vec3(0.3, 0.1, 0)).p()); // set new transform decFemurLatContactGeometry.setTransform(X_GM_latFemur); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int k=0; k < patch.getNumDetails(); ++k) { const ContactDetail& detail = patch.getContactDetail(k); //const Real peakPressure = detail.getPeakPressure(); const Vec3 cp = detail.getContactPoint(); Vec3 newcp = Vec3(cp); // transform point to attach tibia surface Transform cpToTibiaTransf = Transform(newcp); cpToTibiaTransf.setP(cpToTibiaTransf.operator+=( Vec3(0.3,0,0)).p()); DecorativeSphere decCpToTibia; decCpToTibia.setScale(0.0005); decCpToTibia.setTransform(cpToTibiaTransf); decCpToTibia.setColor(Red); // transform point to attach femur surface Transform cpToFemurTransf = Transform(newcp); //Rotation& newRotCpToFemur = cpToFemurTransf.updR(); //newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897); //cpToFemurTransf.set(X_GM_latFemur.R(), X_GM_latFemur.p()); //newRotCpToFemur.operator*=(parentRot_latFemur); //cpToFemurTransf.setP( X_PF_latFemur.p()); //newRotCpToFemur.setRotationFromAngleAboutZ(1.570796326794897); cpToFemurTransf.setP(cpToFemurTransf.operator+=(Vec3(0.3, 0.1, 0)).p()); DecorativeSphere decCpToFemur; decCpToFemur.setScale(0.0005); decCpToFemur.setTransform(cpToFemurTransf); decCpToFemur.setColor(Red); geometry.push_back(decCpToTibia); geometry.push_back(decCpToFemur); if (k == 0) { geometry.push_back(decTibiaContactGeometry); geometry.push_back(decFemurLatContactGeometry); geometry.push_back(decFemurMedContactGeometry); } } } } }
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) override { const Vec3 frcColors[] = {Red,Orange,Cyan}; const Vec3 momColors[] = {Blue,Green,Purple}; m_mbs.realize(state, Stage::Velocity); const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem(); const Real TextScale = 3./16.; m_mbs.realize(state, Stage::Dynamics); const Real KE=m_mbs.calcKineticEnergy(state), E=m_mbs.calcEnergy(state); const Real diss=m_compliant.getDissipatedEnergy(state); const Real PE=m_mbs.calcPotentialEnergy(state); DecorativeText txt; //txt.setIsScreenText(true); //txt.setScale(TextScale); //txt.setColor(Orange); //txt.setText("KE/Diss/E: " + String(KE, "%.6f") + String(diss, "/%.6f") // + String(E+diss, "/%.6f") ); //const Vector qBrick = m_brick.getQAsVector(state); //geometry.push_back(DecorativeText(txt) // .setTransform(Vec3(qBrick[4],0,1))); //geometry.push_back(DecorativeText(txt).setTransform(Vec3(0,0,1))); txt.setColor(Black).setScale(0.8); txt.setText("Ed=" + String(diss, "%0.3f") ); geometry.push_back(DecorativeText(txt).setTransform(Vec3(-6.75,-15,6.75))); txt.setText("Et=" + String(E+diss, "%0.3f") ); geometry.push_back(DecorativeText(txt).setTransform(Vec3(-6.95,-15,5.75))); int nContPts = 0; const int ncont = m_compliant.getNumContactForces(state); if (ncont) { if (!m_inContact) { printf("\n\n--------- NEW CONTACT @%g ----------\n", state.getTime()); m_inContact = true; } } else if (m_inContact) { printf("-------- END OF CONTACT --------\n\n"); m_inContact = false; m_velLines.clear(); } for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); const Vec3& pt = force.getContactPoint(); const Vec3& frc = force.getForceOnSurface2()[1]; const Vec3& mom = force.getForceOnSurface2()[0]; Real frcMag = frc.norm(), momMag=mom.norm(); const UnitVec3 frcDir(frc/frcMag, true); const UnitVec3 momDir(mom/momMag, true); const Vec3 offs = .1*frcDir; // shift up to clear ground //int frcThickness = 2, momThickness = 2; //Real frcScale = ForceScale, momScale = ForceScale; //while (frcMag > /*10*/1000000) // frcThickness++, frcScale /= 10, frcMag /= 10; //while (momMag > /*10*/1000000) // momThickness++, momScale /= 10, momMag /= 10; //geometry.push_back(DecorativePoint(pt) // .setScale(5).setColor(Yellow)); //DecorativeLine frcLine(pt, pt + std::log10(frcMag)*frcDir); //DecorativeLine momLine(pt+offs, pt+offs + std::log10(momMag)*momDir); //frcLine.setColor(Black); //momLine.setColor(Purple); //frcLine.setLineThickness(frcThickness); //momLine.setLineThickness(momThickness); //geometry.push_back(frcLine); //geometry.push_back(momLine); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int i=0; i < patch.getNumDetails(); ++i) { ++nContPts; const ContactDetail& detail = patch.getContactDetail(i); const Vec3& pt = detail.getContactPoint(); //geometry.push_back(DecorativePoint(pt).setColor(Purple)); const Vec3& force = detail.getForceOnSurface2(); const Real forceMag = force.norm(); //const UnitVec3 forceDir(force/forceMag, true); //DecorativeLine frcLine(pt, pt+std::log10(forceMag)*forceDir); //frcLine.setColor(Black); //geometry.push_back(frcLine); // Draw a line that extends from the contact // point in the direction of the slip velocity. const Vec3 v = detail.getSlipVelocity(); const Real vMag = std::max(0., std::log10(v.norm()*1.e3)); const Vec3 vDraw = v.normalize() * vMag; const Vec3 pt0 = Vec3(pt[0], pt[1], 5.e-3); const Vec3 pt1 = Vec3(pt[0]+2.*vDraw[0], pt[1]+2.*vDraw[1], 5.e-3); Real colorFactor = clamp(0.0, m_velLines.size() / 32., 1.0); DecorativeLine slip(pt0, pt1); slip.setLineThickness(3) .setColor(Vec3(1-colorFactor,0.,colorFactor)); // Store line for displaying in subsequent frames, but only if // in compression. if (m_velLines.size() > 0 && v[ZAxis] > 0 && !m_hasCompressionEnded) { m_hasCompressionEnded = true; cout << m_velLines.size() << " lines drawn." << endl; } const bool inCompression = (v[ZAxis] <= 0) && !m_hasCompressionEnded; if (inCompression && vMag>0) m_velLines.push_back(slip); for (int k=0; k<(int)m_velLines.size(); ++k) geometry.push_back(m_velLines[k]); if (i==0 && inCompression && !GenerateAnimation) // REPORT ONLY FIRST CONTACT printf("%8.4f %8.4f %8.4f\n", state.getTime(), v[0], v[1]); } } //txt.setText(String("Num contact points: ") + String(nContPts)); //geometry.push_back(DecorativeText(txt) // .setTransform(Vec3(state.getQ()[4],0,.75))); txt.setText("t=" + String(state.getTime(), "%0.3f") + "s"); txt.setColor(Black).setScale(0.8); geometry.push_back(DecorativeText(txt).setTransform(Vec3(-7.5,-15,7.75))); if (nContPts>0) { txt.setText(String("Contacting")); geometry.push_back(DecorativeText(txt).setTransform(Vec3(9,-15,7.75))); } }
virtual void generateDecorations(const State& state, Array_<DecorativeGeometry>& geometry) { const Vec3 frcColors[] = {Red,Orange,Cyan}; const Vec3 momColors[] = {Blue,Green,Purple}; m_mbs.realize(state, Stage::Velocity); const SimbodyMatterSubsystem& matter = m_mbs.getMatterSubsystem(); const Real TextScale = m_mbs.getDefaultLengthScale()/10; // was .1 m_mbs.realize(state, Stage::Dynamics); const Real KE=m_mbs.calcKineticEnergy(state), E=m_mbs.calcEnergy(state); const Real diss=m_compliant.getDissipatedEnergy(state); DecorativeText txt; txt.setIsScreenText(true); txt.setText("KE/Diss/E: " + String(KE, "%.6f") + String(diss, "/%.6f") + String(E+diss, "/%.6f") ); geometry.push_back(txt); int nContPts = 0; const int ncont = m_compliant.getNumContactForces(state); for (int i=0; i < ncont; ++i) { const ContactForce& force = m_compliant.getContactForce(state,i); const ContactId id = force.getContactId(); const Vec3& pt = force.getContactPoint(); const Vec3& frc = force.getForceOnSurface2()[1]; const Vec3& mom = force.getForceOnSurface2()[0]; Real frcMag = frc.norm(), momMag=mom.norm(); const UnitVec3 frcDir(frc/frcMag, true); const UnitVec3 momDir(mom/momMag, true); const Vec3 offs = .1*frcDir; // shift up to clear ground int frcThickness = 2, momThickness = 2; Real frcScale = ForceScale, momScale = ForceScale; while (frcMag > /*10*/1000000) frcThickness++, frcScale /= 10, frcMag /= 10; while (momMag > /*10*/1000000) momThickness++, momScale /= 10, momMag /= 10; geometry.push_back(DecorativePoint(pt) .setScale(5).setColor(Yellow)); DecorativeLine frcLine(pt, pt + std::log10(frcMag)*frcDir); DecorativeLine momLine(pt+offs, pt+offs + std::log10(momMag)*momDir); frcLine.setColor(Black); momLine.setColor(Purple); frcLine.setLineThickness(frcThickness); momLine.setLineThickness(momThickness); geometry.push_back(frcLine); geometry.push_back(momLine); ContactPatch patch; const bool found = m_compliant.calcContactPatchDetailsById(state,id,patch); //cout << "patch for id" << id << " found=" << found << endl; //cout << "resultant=" << patch.getContactForce() << endl; //cout << "num details=" << patch.getNumDetails() << endl; for (int i=0; i < patch.getNumDetails(); ++i) { ++nContPts; const ContactDetail& detail = patch.getContactDetail(i); const Vec3& pt = detail.getContactPoint(); geometry.push_back(DecorativePoint(pt).setColor(Purple)); const Vec3& force = detail.getForceOnSurface2(); const Real forceMag = force.norm(); const UnitVec3 forceDir(force/forceMag, true); DecorativeLine frcLine(pt, pt+std::log10(forceMag)*forceDir); frcLine.setColor(Black); geometry.push_back(frcLine); // Make a red line that extends from the contact // point in the direction of the slip velocity, of length 3*slipvel. DecorativeLine slip(pt,pt+3.*detail.getSlipVelocity()); slip.setColor(Red); geometry.push_back(slip); } } txt.setText(String("Num contact points: ") + String(nContPts)); geometry.push_back(txt); }