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