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