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);
}
}
}
}
}
ContactGeometry::TriangleMesh::Impl::Impl
(const PolygonalMesh& mesh, bool smooth)
: ContactGeometryImpl(), smooth(smooth)
{ // Create the mesh, triangulating faces as necessary.
Array_<Vec3> vertexPositions;
Array_<int> faceIndices;
for (int i = 0; i < mesh.getNumVertices(); i++)
vertexPositions.push_back(mesh.getVertexPosition(i));
for (int i = 0; i < mesh.getNumFaces(); i++) {
int numVert = mesh.getNumVerticesForFace(i);
if (numVert < 3)
continue; // Ignore it.
if (numVert == 3) {
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(mesh.getFaceVertex(i, 1));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
}
else if (numVert == 4) {
// Split it into two triangles.
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(mesh.getFaceVertex(i, 1));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
faceIndices.push_back(mesh.getFaceVertex(i, 2));
faceIndices.push_back(mesh.getFaceVertex(i, 3));
faceIndices.push_back(mesh.getFaceVertex(i, 0));
}
else {
// Add a vertex at the center, then split it into triangles.
Vec3 center(0);
for (int j = 0; j < numVert; j++)
center += vertexPositions[mesh.getFaceVertex(i, j)];
center /= numVert;
vertexPositions.push_back(center);
int newIndex = vertexPositions.size()-1;
for (int j = 0; j < numVert-1; j++) {
faceIndices.push_back(mesh.getFaceVertex(i, j));
faceIndices.push_back(mesh.getFaceVertex(i, j+1));
faceIndices.push_back(newIndex);
}
// Close the face (thanks, Alexandra Zobova).
faceIndices.push_back(mesh.getFaceVertex(i, numVert-1));
faceIndices.push_back(mesh.getFaceVertex(i, 0));
faceIndices.push_back(newIndex);
}
}
init(vertexPositions, faceIndices);
// Make sure the mesh normals are oriented correctly.
Vec3 origin(0);
for (int i = 0; i < 3; i++)
origin += vertices[faces[0].vertices[i]].pos;
origin /= 3; // this is the face centroid
const UnitVec3 direction = -faces[0].normal;
// Calculate a ray origin that is guaranteed to be outside the
// mesh. If the topology is right (face 0 normal points outward), we'll be
// outside on the side containing face 0. If it is wrong, we'll be outside
// on the opposite side of the mesh. Then we'll shoot a ray back along the
// direction we came from (that is, towards the interior of the mesh from
// outside). We'll hit *some* face. If the topology is right, the hit
// face's normal will be pointing back at us. If it is wrong, the face
// normal will also be pointing inwards, in roughly the same direction as
// the ray.
origin -= max(obb.bounds.getSize())*direction;
Real distance;
int face;
Vec2 uv;
bool intersects = intersectsRay(origin, direction, distance, face, uv);
assert(intersects);
// Now dot the hit face normal with the ray direction; correct topology
// will have them pointing in more-or-less opposite directions.
if (dot(faces[face].normal, direction) > 0) {
// We need to invert the mesh topology.
for (int i = 0; i < (int) faces.size(); i++) {
Face& f = faces[i];
int temp = f.vertices[0];
f.vertices[0] = f.vertices[1];
f.vertices[1] = temp;
temp = f.edges[1];
f.edges[1] = f.edges[2];
f.edges[2] = temp;
f.normal *= -1;
}
for (int i = 0; i < (int) vertices.size(); i++)
vertices[i].normal *= -1;
}
}
int main() {
try {
// Currently, this only tests a small number of operations that were recently added.
// It should be expanded into a more comprehensive test of the big matrix classes.
// Test matrix elementwise initialization.
Matrix minit(2,3, 5.25);
testMatrix<Matrix,2,3>(minit, Mat23(5.25, 5.25, 5.25,
5.25, 5.25, 5.25));
const Vec2 v12(1,2);
Matrix_<Vec2> mvinit(3,2, v12);
testMatrix<Matrix_<Vec2>,3,2>(mvinit, Mat<3,2,Vec2>(v12,v12,
v12,v12,
v12,v12));
testMatDivision();
testTransform();
Matrix m(Mat22(1, 2, 3, 4));
testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4));
m += 3;
testMatrix<Matrix,2,2>(m, Mat22(4, 2, 3, 7));
m -= 3;
testMatrix<Matrix,2,2>(m, Mat22(1, 2, 3, 4));
testMatrix<Matrix,2,2>(m-1, Mat22(0, 2, 3, 3));
testMatrix<Matrix,2,2>(m+1, Mat22(2, 2, 3, 5));
testMatrix<Matrix,2,2>(1-m, Mat22(0, -2, -3, -3));
testMatrix<Matrix,2,2>(1+m, Mat22(2, 2, 3, 5));
Vector v(Vec3(1, 2, 3));
testVector(v, Vec3(1, 2, 3));
v += 2;
testVector(v, Vec3(3, 4, 5));
v -= 2;
testVector(v, Vec3(1, 2, 3));
testVector(v-1, Vec3(0, 1, 2));
testVector(v+1, Vec3(2, 3, 4));
testVector(1-v, Vec3(0, -1, -2));
testVector(1+v, Vec3(2, 3, 4));
RowVector r(Row3(1, 2, 3));
testVector(r, Vec3(1, 2, 3));
r += 2;
testVector(r, Vec3(3, 4, 5));
r -= 2;
testVector(r, Vec3(1, 2, 3));
testVector(r-1, Vec3(0, 1, 2));
testVector(r+1, Vec3(2, 3, 4));
testVector(1-r, Vec3(0, -1, -2));
testVector(1+r, Vec3(2, 3, 4));
Matrix mm( Mat23( 1, 2, 3,
7, 8, 9 ) );
testMatrix<Matrix,2,3>(mm, Mat23(1,2,3,7,8,9));
// Test copying a column or row of a Matrix into
// a Vector or RowVector.
// Test assignment constructor
Vector vv = mm(1); testVector(vv, Vec2(2,8));
// Test copy assignment
vv = mm(0); testVector(vv, Vec2(1,7));
// Test assignment constructor
RowVector rr = mm[1]; testVector(rr, Vec3(7,8,9));
// Test copy assignment
rr = mm[0]; testVector(rr, Vec3(1,2,3));
// Test copying a row into a Vector and column into RowVector.
// Test assignment (copy) constructor
RowVector rrr = ~mm(1);
testVector(rrr, Vec2(2,8));
// Test copy assignment
rrr = ~mm(0); testVector(rrr, Vec2(1,7));
// Test assignment (copy) constructor
Vector vvv = ~mm[1];
testVector(vvv, Vec3(7,8,9));
// Test copy assignment
vvv = ~mm[0]; testVector(vvv, Vec3(1,2,3));
// Test creating a Matrix that shares space with an Array
// Easy case: sizeof(element) == sizeof(scalar)
Array_<Real> rarrmat;
rarrmat.push_back(1.1); rarrmat.push_back(2.2); // col(0)
rarrmat.push_back(3.3); rarrmat.push_back(4.4); // col(1)
Matrix rmatrix(2,2, 2/*lda*/, &rarrmat[0]);
testMatrix<Matrix,2,2>(rmatrix, Mat22(1.1, 3.3,
2.2, 4.4));
// Here sizeof(element) != sizeof(scalar)
Array_<SpatialVec> svarrmat;
svarrmat.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6)));
svarrmat.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1)));
svarrmat.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2)));
svarrmat.push_back(SpatialVec(Vec3(1.3,2.3,3.3),Vec3(4.3,5.3,6.3)));
const int szInScalars = sizeof(SpatialVec)/sizeof(Real);
Matrix_<SpatialVec> svmatrix(2,2, 2*szInScalars/*lda*/,
(Real*)&svarrmat[0]);
Matrix_<SpatialVec> svmatans(2,2);
svmatans(0,0) = svarrmat[0]; svmatans(1,0)=svarrmat[1];
svmatans(0,1) = svarrmat[2]; svmatans(1,1)=svarrmat[3];
SimTK_TEST_EQ_TOL(svmatrix, svmatans, 1e-16); // should be exact
// Test creating a Vector that shares space with an Array
// Easy case: sizeof(element) == sizeof(scalar)
Array_<Real> rarray;
rarray.push_back(1.1); rarray.push_back(2.2); rarray.push_back(3.3);
Vector rvector(3, &rarray[0], true);
testVector(rarray, Vec3(1.1,2.2,3.3));
// Here sizeof(element) != sizeof(scalar)
Array_<SpatialVec> svarray;
svarray.push_back(SpatialVec(Vec3(1,2,3),Vec3(4,5,6)));
svarray.push_back(SpatialVec(Vec3(1.1,2.1,3.1),Vec3(4.1,5.1,6.1)));
svarray.push_back(SpatialVec(Vec3(1.2,2.2,3.2),Vec3(4.2,5.2,6.2)));
Vector_<SpatialVec> svvector(3, (Real*)&svarray[0], true);
Vector_<SpatialVec> svanswer(3);
svanswer[0]=svarray[0];svanswer[1]=svarray[1];svanswer[2]=svarray[2];
SimTK_TEST_EQ_TOL(svvector, svanswer, 1e-16); // should be exact
// Create 0-width slices of Matrix that has general shape,
// vector shape, and row vector shape. This caused trouble before
// because vector and row shapes use 1d matrix storage; when making
// a 0-width slice of those they have to go back to general shape.
// Note that you are allowed to index off the bottom and right if
// you make a zero-width slice.
Matrix general(3, 4);
MatrixView gslice1 = general(1,1,0,2); // middle
SimTK_TEST(gslice1.nrow()==0 && gslice1.ncol()==2);
MatrixView gslice2 = general(1,1,1,0); // middle
SimTK_TEST(gslice2.nrow()==1 && gslice2.ncol()==0);
MatrixView gslice3 = general(0,0,3,0); // left side
SimTK_TEST(gslice3.nrow()==3 && gslice3.ncol()==0);
MatrixView gslice4 = general(0,0,0,4); // top
SimTK_TEST(gslice4.nrow()==0 && gslice4.ncol()==4);
MatrixView gslice5 = general(3,0,0,4); // off the bottom
SimTK_TEST(gslice5.nrow()==0 && gslice5.ncol()==4);
MatrixView gslice6 = general(0,4,3,0); // off the right side
SimTK_TEST(gslice6.nrow()==3 && gslice6.ncol()==0);
MatrixView gslice7 = general(0,0,0,0);
SimTK_TEST(gslice7.nrow()==0 && gslice7.ncol()==0);
MatrixView gslice8 = general(1,2,0,0);
SimTK_TEST(gslice8.nrow()==0 && gslice8.ncol()==0);
MatrixView gslice9 = general(2,3,0,0);
SimTK_TEST(gslice9.nrow()==0 && gslice9.ncol()==0);
MatrixView vector = general(0,1,3,1);
SimTK_TEST(vector.nrow()==3 && vector.ncol()==1);
MatrixView vslice1 = vector(0,0,3,0);
SimTK_TEST(vslice1.nrow()==3 && vslice1.ncol()==0);
MatrixView vslice2 = vector(0,0,0,0);
SimTK_TEST(vslice2.nrow()==0 && vslice2.ncol()==0);
MatrixView vslice3 = vector(2,0,1,0);
SimTK_TEST(vslice3.nrow()==1 && vslice3.ncol()==0);
MatrixView vslice4 = vector(3,0,0,1); // off the bottom
SimTK_TEST(vslice4.nrow()==0 && vslice4.ncol()==1);
MatrixView vslice5 = vector(0,1,3,0); // off the right
SimTK_TEST(vslice5.nrow()==3 && vslice5.ncol()==0);
vslice5 = Matrix(3,0);
} catch(const std::exception& e) {
cout << "exception: " << e.what() << endl;
return 1;
}
cout << "Done" << endl;
return 0;
}
// Call this on a newly-constructed ModelVisualizer (typically from the Model's
// initSystem() method) to set up the various auxiliary classes used for
// visualization and user interaction. This involves modifications to the
// System that must be done prior to realizeTopology(), and may modify the
// Model also.
void ModelVisualizer::createVisualizer() {
_model.updMatterSubsystem().setShowDefaultGeometry(false);
// Allocate a Simbody Visualizer. If environment variable
// OPENSIM_HOME is set, add its bin subdirectory to the search path
// for the SimbodyVisualizer executable. The search will go as
// follows: first look in the same directory as the currently-
// executing executable; then look in the $OPENSIM_HOME/bin
// directory, then look in various default Simbody places.
Array_<String> searchPath;
if (SimTK::Pathname::environmentVariableExists("OPENSIM_HOME")) {
searchPath.push_back(
SimTK::Pathname::getEnvironmentVariable("OPENSIM_HOME")
+ "/bin");
}
_viz = new SimTK::Visualizer(_model.getMultibodySystem(),
searchPath);
// Make the Simbody Visualizer (that is, the display window) kill itself
// when the API-side connection is lost (because the Visualizer object gets
// destructed). Otherwise it will hang around afterwards.
_viz->setShutdownWhenDestructed(true);
_viz->setCameraClippingPlanes(.01,100.);
_viz->setBackgroundColor(SimTK::Black);
_viz->setBackgroundType(SimTK::Visualizer::SolidColor);
// Give it an OpenSim-friendly window heading.
bool isAbsolutePath; string directory, fileName, extension;
SimTK::Pathname::deconstructPathname(
SimTK::Pathname::getThisExecutablePath(),
isAbsolutePath, directory, fileName, extension);
_viz->setWindowTitle("OpenSim " + OpenSim::GetVersion()
+ ": " + fileName + " (" + _model.getName() + ")");
// Create a menu for choosing what to display.
SimTK::Array_< std::pair<SimTK::String, int> > selections;
selections.push_back(std::make_pair("Wrap geometry",
ToggleWrapGeometry));
selections.push_back(std::make_pair("Contact geometry",
ToggleContactGeometry));
selections.push_back(std::make_pair("Muscle paths",ToggleMusclePaths));
selections.push_back(std::make_pair("Path points",TogglePathPoints));
selections.push_back(std::make_pair("Markers",ToggleMarkers));
selections.push_back(std::make_pair("Frames",ToggleFrames));
selections.push_back(std::make_pair("Default geometry",
ToggleDefaultGeometry));
_viz->addMenu("Show", ShowMenuId, selections);
// Add a DecorationGenerator to dispatch runtime generateDecorations()
// calls.
_decoGen = new DefaultGeometry(_model);
_viz->addDecorationGenerator(_decoGen);
// Add an input listener to handle display menu picks.
_viz->addInputListener(new OpenSimInputListener(_model));
// Allocate an InputSilo to pick up anything the above listener doesn't.
_silo = new SimTK::Visualizer::InputSilo();
_viz->addInputListener(_silo);
// This is used for regular output of frames during forward dynamics.
// TODO: allow user control of timing.
_model.updMultibodySystem().addEventReporter
(new SimTK::Visualizer::Reporter(*_viz, 1./30));
}
VisualizerProtocol::VisualizerProtocol
(Visualizer& visualizer, const Array_<String>& userSearchPath)
{
// Launch the GUI application. We'll first look for one in the same
// directory as the running executable; then if that doesn't work we'll
// look in the bin subdirectory of the SimTK installation.
String vizExecutableName;
if (Pathname::environmentVariableExists("SIMBODY_VISUALIZER_NAME")) {
vizExecutableName =
Pathname::getEnvironmentVariable("SIMBODY_VISUALIZER_NAME");
} else {
vizExecutableName = "simbody-visualizer";
#ifndef NDEBUG
vizExecutableName += "_d";
#endif
}
Array_<String> actualSearchPath;
// Always start with the current executable's directory.
actualSearchPath.push_back(Pathname::getThisExecutableDirectory());
// User's stuff comes next, if any directories were provided. We're going
// to turn these into absolute pathnames, interpreting them as defined
// by Pathname, which includes executable-relative names. The "bin"
// subdirectory if any must already be present in the directory names.
for (unsigned i=0; i < userSearchPath.size(); ++i)
actualSearchPath.push_back
(Pathname::getAbsoluteDirectoryPathname(userSearchPath[i]));
if (Pathname::environmentVariableExists("SIMBODY_HOME")) {
const std::string e = Pathname::getAbsoluteDirectoryPathname(
Pathname::getEnvironmentVariable("SIMBODY_HOME"));
actualSearchPath.push_back(Pathname::addDirectoryOffset(e,
SIMBODY_VISUALIZER_REL_INSTALL_DIR));
} else if (Pathname::environmentVariableExists("SimTK_INSTALL_DIR")) {
const std::string e = Pathname::getAbsoluteDirectoryPathname(
Pathname::getEnvironmentVariable("SimTK_INSTALL_DIR"));
actualSearchPath.push_back(Pathname::addDirectoryOffset(e,
SIMBODY_VISUALIZER_REL_INSTALL_DIR));
}
// Try using the location of SimTKsimbody combined with the path from the
// SimTKsimbody library to the simbody-visualizer install location (only
// available on non-Windows platforms). We must provide a function that
// resides in SimTKsimbody.
std::string SimTKsimbodyDir;
if (Pathname::getFunctionLibraryDirectory((void*)createPipeSim2Viz,
SimTKsimbodyDir)) {
// We have the path to SimTKsimbody; now we combine this with the path
// from SimTKsimbody to simbody-visualizer (this assumes the
// installation has not been reorganized from what CMake originally
// specified).
std::string absPathToVizDir =
Pathname::getAbsoluteDirectoryPathnameUsingSpecifiedWorkingDirectory(
SimTKsimbodyDir, SIMBODY_PATH_FROM_LIBDIR_TO_VIZ_DIR);
actualSearchPath.push_back(absPathToVizDir);
}
// Try the build-time install location:
actualSearchPath.push_back(SIMBODY_VISUALIZER_INSTALL_DIR);
// Our last desperate attempts will
// be <platformDefaultInstallDir>/Simbody/bin
// and <platformDefaultInstallDir>/SimTK/bin
const std::string def = Pathname::getDefaultInstallDir();
actualSearchPath.push_back(
Pathname::addDirectoryOffset(def,
Pathname::addDirectoryOffset("Simbody", SIMBODY_VISUALIZER_REL_INSTALL_DIR)));
actualSearchPath.push_back(
Pathname::addDirectoryOffset(def,
Pathname::addDirectoryOffset("SimTK", SIMBODY_VISUALIZER_REL_INSTALL_DIR)));
// Pipe[0] is the read end, Pipe[1] is the write end.
int sim2vizPipe[2], viz2simPipe[2], status;
// Create pipe pair for communication from simulator to visualizer.
status = createPipeSim2Viz(sim2vizPipe);
SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
outPipe = sim2vizPipe[1]; // write here to talk to visualizer
// Create pipe pair for communication from visualizer to simulator.
status = createPipeViz2Sim(viz2simPipe);
SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
inPipe = viz2simPipe[0]; // read from here to receive from visualizer
// Spawn the visualizer gui, trying local first then installed version.
spawnViz(actualSearchPath, vizExecutableName, sim2vizPipe, viz2simPipe);
// Before we do anything else, attempt to exchange handshake messages with
// the visualizer. This will throw an exception if anything goes wrong.
// Note that this is done on the main thread.
shakeHandsWithGUI(outPipe, inPipe);
// Spawn the thread to listen for events.
pthread_mutex_init(&sceneLock, NULL);
pthread_create(&eventListenerThread, NULL, listenForVisualizerEvents,
&visualizer);
}
VisualizerProtocol::VisualizerProtocol
(Visualizer& visualizer, const Array_<String>& userSearchPath)
{
// Launch the GUI application. We'll first look for one in the same directory
// as the running executable; then if that doesn't work we'll look in the
// bin subdirectory of the SimTK installation.
const char* GuiAppName = "simbody-visualizer";
Array_<String> actualSearchPath;
// Always start with the current executable's directory.
actualSearchPath.push_back(Pathname::getThisExecutableDirectory());
// User's stuff comes next, if any directories were provided. We're going
// to turn these into absolute pathnames, interpreting them as defined
// by Pathname, which includes executable-relative names. The "bin"
// subdirectory if any must already be present in the directory names.
for (unsigned i=0; i < userSearchPath.size(); ++i)
actualSearchPath.push_back
(Pathname::getAbsoluteDirectoryPathname(userSearchPath[i]));
if (Pathname::environmentVariableExists("SIMBODY_HOME")) {
const std::string e = Pathname::getAbsoluteDirectoryPathname(
Pathname::getEnvironmentVariable("SIMBODY_HOME"));
actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin"));
} else if (Pathname::environmentVariableExists("SimTK_INSTALL_DIR")) {
const std::string e = Pathname::getAbsoluteDirectoryPathname(
Pathname::getEnvironmentVariable("SimTK_INSTALL_DIR"));
actualSearchPath.push_back(Pathname::addDirectoryOffset(e,"bin"));
}
// Try the build-time install location:
actualSearchPath.push_back(SIMBODY_VISUALIZER_INSTALL_DIR);
// Our last desperate attempts will
// be <platformDefaultInstallDir>/Simbody/bin
// and <platformDefaultInstallDir>/SimTK/bin
const std::string def = Pathname::getDefaultInstallDir();
actualSearchPath.push_back(
Pathname::addDirectoryOffset(def,
Pathname::addDirectoryOffset("Simbody", "bin")));
actualSearchPath.push_back(
Pathname::addDirectoryOffset(def,
Pathname::addDirectoryOffset("SimTK", "bin")));
// Pipe[0] is the read end, Pipe[1] is the write end.
int sim2vizPipe[2], viz2simPipe[2], status;
// Create pipe pair for communication from simulator to visualizer.
status = createPipeSim2Viz(sim2vizPipe);
SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
outPipe = sim2vizPipe[1]; // write here to talk to visualizer
// Create pipe pair for communication from visualizer to simulator.
status = createPipeViz2Sim(viz2simPipe);
SimTK_ASSERT_ALWAYS(status != -1, "VisualizerProtocol: Failed to open pipe");
inPipe = viz2simPipe[0]; // read from here to receive from visualizer
// Spawn the visualizer gui, trying local first then installed version.
spawnViz(actualSearchPath, GuiAppName, sim2vizPipe, viz2simPipe);
// Before we do anything else, attempt to exchange handshake messages with
// the visualizer. This will throw an exception if anything goes wrong.
// Note that this is done on the main thread.
shakeHandsWithGUI(outPipe, inPipe);
// Spawn the thread to listen for events.
pthread_mutex_init(&sceneLock, NULL);
pthread_t thread;
pthread_create(&thread, NULL, listenForVisualizerEvents, &visualizer);
}
int constraintJacobian(const Vector& parameters,
bool new_parameters,
Matrix& J) const override
{ ++nEvalJacobian;
if (new_parameters)
setInternalStateFromFreeQs(parameters);
for (unsigned i=0; i < assembler.reporters.size(); ++i)
assembler.reporters[i]->handleEvent(getInternalState());
assert(J.nrow() == getNumEqualityConstraints());
assert(J.ncol() == getNumFreeQs());
const int n = getNumFreeQs();
// This will record the indices of any constraints we encounter that
// can't provide their own gradients; we'll handle them all together
// at the end.
Array_<AssemblyConditionIndex> needNumericalJacobian;
Array_<int> firstEqn;
Array_<int> nEqns;
int needy = 0;
int nxtEqn = 0;
for (unsigned i=0; i < assembler.errors.size(); ++i) {
AssemblyConditionIndex consIx = assembler.errors[i];
const AssemblyCondition& cond = *assembler.conditions[consIx];
const int m = cond.getNumErrors(getInternalState());
const int stat = (assembler.forceNumericalJacobian
? -1
: cond.calcErrorJacobian(getInternalState(),
J(nxtEqn,0,m,n)));
if (stat == -1) {
needNumericalJacobian.push_back(consIx);
firstEqn.push_back(nxtEqn);
nEqns.push_back(m);
needy += m;
} else if (stat != 0)
return stat;
nxtEqn += m;
}
if (!needNumericalJacobian.empty()) {
//cout << "Need numerical Jacobian for "
// << needNumericalJacobian.size() << " constraints." << endl;
NumJacobianFunc numCons(assembler, needNumericalJacobian,
nEqns, needy);
// Forward difference should be fine here, unlike for the
// gradient because we converge on the solution value
// rather than the derivative norm.
Differentiator jacNumCons(numCons);
Matrix numJ = jacNumCons.calcJacobian(getFreeQsFromInternalState());
nEvalConstraints += jacNumCons.getNumCallsToUserFunction();
// Fill in the missing rows.
int nxtInNumJ = 0;
for (unsigned i=0; i < needNumericalJacobian.size(); ++i) {
J(firstEqn[i],0,nEqns[i],n) = numJ(nxtInNumJ,0,nEqns[i],n);
nxtInNumJ += nEqns[i];
}
}
//cout << "J=" << J;
return 0;
}
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);
}
int main() {
try { // catch errors if any
// Create the system, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.8);
system.setUseUniformBackground(true); // request no ground & sky
// Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
Real mass = 3; Vec3 pos(0,-3,0);
Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));
// Create the tree of mobilized bodies, reusing the above body description.
MobilizedBody::Pin bodyT (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0), bodyInfo, Vec3(0));
MobilizedBody::Pin rtArm (bodyT, Vec3(2, 0, 0), bodyInfo, Vec3(0,-1,0));
// Add some damping.
Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10);
Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30);
Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10);
#ifdef USE_TORQUE_LIMITED_MOTOR
MyTorqueLimitedMotor* motorp =
new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque);
const MyTorqueLimitedMotor& motor = *motorp;
Force::Custom(forces, motorp); // takes over ownership
#else
// Use built-in Steady Motion as a low-budget motor model.
//Motion::Steady motor(rtArm, InitialMotorRate);
// Use built-in ConstantSpeed constraint as a low-budget motor model.
Constraint::ConstantSpeed motor(rtArm, InitialMotorRate);
#endif
// Add a joint stop to the left arm restricting it to q in [0,Pi/5].
Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
StopStiffness,
InitialDissipation,
-Pi/8, // lower stop
Pi/8); // upper stop
Visualizer viz(system);
// Add sliders.
viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate);
viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation);
viz.addSlider("Tach", SliderIdTach, -20, 20, 0);
viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0);
// Add Run menu.
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Reset", ResetItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.addMenu("Run", MenuIdRun, runMenuItems);
Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
viz.addInputListener(userInput);
// Initialize the system and state.
State initState = system.realizeTopology();
// Simulate forever with a small max step size. Check for user input
// in between steps. Note: an alternate way to do this is to let the
// integrator take whatever steps it wants but use a TimeStepper to
// manage a periodic event handler to poll for user input. Here we're
// treating completion of a step as an event.
const Real MaxStepSize = 0.01*3; // 10ms
const int DrawEveryN = 3/3; // 3 steps = 30ms
//RungeKuttaMersonIntegrator integ(system);
//RungeKutta2Integrator integ(system);
SemiExplicitEuler2Integrator integ(system);
//SemiExplicitEulerIntegrator integ(system, .001);
integ.setAccuracy(1e-1);
//integ.setAccuracy(1e-3);
// Don't permit interpolation because we want the state returned after
// a step to be modifiable.
integ.setAllowInterpolation(false);
integ.initialize(initState);
int stepsSinceViz = DrawEveryN-1;
while (true) {
if (++stepsSinceViz % DrawEveryN == 0) {
const State& s = integ.getState();
viz.report(s);
const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0));
viz.setSliderValue(SliderIdTach, uActual);
#ifdef USE_TORQUE_LIMITED_MOTOR
viz.setSliderValue(SliderIdTorque, motor.getTorque(s));
#else
system.realize(s); // taus are acceleration stage
//viz.setSliderValue(SliderIdTorque,
// rtArm.getOneTau(s, MobilizerUIndex(0)));
viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s));
#endif
stepsSinceViz = 0;
}
// Advance time by MaxStepSize (might take multiple steps to get there).
integ.stepBy(MaxStepSize);
// Now poll for user input.
int whichSlider, whichMenu, whichItem; Real newValue;
// Did a slider move?
if (userInput->takeSliderMove(whichSlider, newValue)) {
State& state = integ.updAdvancedState();
switch(whichSlider) {
case SliderIdMotorSpeed:
// TODO: momentum balance?
//motor.setRate(state, newValue);
motor.setSpeed(state, newValue);
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
break;
case SliderIdDissipation:
stop.setMaterialProperties(state, StopStiffness, newValue);
system.realize(state, Stage::Position);
break;
}
}
// Was there a menu pick?
if (userInput->takeMenuPick(whichMenu, whichItem)) {
if (whichItem == QuitItem)
break; // done
// If Reset, stop the motor and restore default dissipation.
// Tell visualizer to update the sliders to match.
// Zero out all the q's and u's.
if (whichItem == ResetItem) {
State& state = integ.updAdvancedState();
//motor.setRate(state, 0);
motor.setSpeed(state, 0);
viz.setSliderValue(SliderIdMotorSpeed, 0);
stop.setMaterialProperties(state, StopStiffness, InitialDissipation);
viz.setSliderValue(SliderIdDissipation, InitialDissipation);
state.updQ() = 0; // all positions to zero
state.updU() = 0; // all velocities to zero
system.realize(state, Stage::Position);
system.prescribeU(state);
system.realize(state, Stage::Velocity);
system.projectU(state);
}
}
}
const int evals = integ.getNumRealizations();
std::cout << "Done -- simulated " << integ.getTime() << "s with "
<< integ.getNumStepsTaken() << " steps, avg step="
<< (1000*integ.getTime())/integ.getNumStepsTaken() << "ms "
<< (1000*integ.getTime())/evals << "ms/eval\n";
printf("Used Integrator %s at accuracy %g:\n",
integ.getMethodName(), integ.getAccuracyInUse());
printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
void handleEvent(const State& state) const {
saveEm.push_back(state);
}
int main() {
try
{ // Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, UnitVec3(.1,-1,0), 9.81);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
contactForces.setTransitionVelocity(1e-3);
const Vec3 hdim(1,2,3);
const Real fFac =.15; // to turn off friction
const Real fDis = .5; // to turn off dissipation
const Real fVis = .1; // to turn off viscous friction
const Real fK = .1*1e6; // pascals
// Halfspace floor
const Rotation R_xdown(-Pi/2,ZAxis);
matter.Ground().updBody().addDecoration(
Transform(Vec3(0,-.5,0)),
DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1));
matter.Ground().updBody().addContactSurface(
Transform(R_xdown, Vec3(0,0,0)),
ContactSurface(ContactGeometry::HalfSpace(),
ContactMaterial(fK*.1,fDis*.9,
fFac*.8,fFac*.7,fVis*10)));
const Real brickMass = 10;
Body::Rigid brickBody(MassProperties(brickMass, Vec3(0),
UnitInertia::brick(hdim)));
brickBody.addDecoration(Transform(),
DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3));
const int surfx = brickBody.addContactSurface(Transform(),
ContactSurface(ContactGeometry::Brick(hdim),
ContactMaterial(fK,fDis,
fFac*.8,fFac*.7,fVis))
);
//brickBody.addContactSurface(Transform(),
// ContactSurface(ContactGeometry::Ellipsoid(hdim),
// ContactMaterial(fK*.1,fDis*.9,
// .1*fFac*.8,.1*fFac*.7,fVis*1))
// );
const ContactSurface& surf = brickBody.getContactSurface(surfx);
const ContactGeometry& cg = surf.getShape();
const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg);
cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl;
const Geo::Box& box = cgbrick.getGeoBox();
cout << "box.hdim=" << box.getHalfLengths() << endl;
// Vertices
for (int i=0; i<8; ++i) {
const Vec3 vpos = box.getVertexPos(i);
const UnitVec3 vn = box.getVertexNormal(i);
brickBody.addDecoration
(DecorativePoint(vpos).setColor(Orange));
brickBody.addDecoration
(DecorativeText(String(i)).setTransform(vpos).setColor(White)
.setScale(.5));
brickBody.addDecoration
(DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange));
printf("vertex %d:\n", i);
int e[3],ew[3],f[3],fw[3];
box.getVertexEdges(i,e,ew);
box.getVertexFaces(i,f,fw);
for (int ex=0; ex<3; ++ex) {
int ev[2]; box.getEdgeVertices(e[ex], ev);
printf(" e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]);
}
for (int fx=0; fx<3; ++fx) {
int fv[4]; box.getFaceVertices(f[fx], fv);
printf(" f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]);
}
}
// Edges
for (int i=0; i<12; ++i) {
const UnitVec3 n = box.getEdgeNormal(i);
const UnitVec3 d = box.getEdgeDirection(i);
const Vec3 ctr = box.getEdgeCenter(i);
const Real len = .75;
brickBody.addDecoration
(DecorativePoint(ctr).setColor(Green).setScale(2));
brickBody.addDecoration
(DecorativeText(String(i)).setTransform(ctr+len*n)
.setColor(Green).setScale(.3));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + len*n).setColor(Green));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + len*d).setColor(Green));
printf("edge %d:\n", i);
int f[2],fw[2];
box.getEdgeFaces(i,f,fw);
for (int fx=0; fx<2; ++fx) {
int fe[4]; box.getFaceEdges(f[fx], fe);
printf(" f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]);
}
}
// Faces
for (int i=0; i<6; ++i) {
int vertices[4]; box.getFaceVertices(i,vertices);
const UnitVec3 n = box.getFaceNormal(i);
const Vec3 ctr = box.getFaceCenter(i);
brickBody.addDecoration
(DecorativePoint(ctr).setColor(Magenta).setScale(3));
brickBody.addDecoration
(Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr),
DecorativeText(String(i)).setColor(Magenta)
.setScale(.75).setFaceCamera(false));
brickBody.addDecoration
(DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta));
}
MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)),
brickBody, Transform(Vec3(0)));
Visualizer viz(system);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
viz.setShowShadows(true);
viz.setShowSimTime(true);
viz.setDesiredFrameRate(FrameRate);
viz.setShowFrameRate(true);
viz.setBackgroundType(Visualizer::SolidColor);
viz.setBackgroundColor(White*.9);
Visualizer::InputSilo* silo = new Visualizer::InputSilo();
viz.addInputListener(silo);
Array_<std::pair<String,int> > runMenuItems;
runMenuItems.push_back(std::make_pair("Go", GoItem));
runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
runMenuItems.push_back(std::make_pair("Quit", QuitItem));
viz.addMenu("Run", RunMenuId, runMenuItems);
Array_<std::pair<String,int> > helpMenuItems;
helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
viz.addMenu("Help", HelpMenuId, helpMenuItems);
system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
// Check for a Run->Quit menu pick every 1/4 second.
system.addEventHandler(new UserInputHandler(*silo, .25));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
brick.setQToFitRotation(state, Rotation(SpaceRotationSequence,
.1, ZAxis, .05, XAxis));
brick.setUToFitLinearVelocity(state, Vec3(2,0,0));
saveEm.reserve(10000);
viz.report(state);
printf("Default state\n");
cout << "t=" << state.getTime()
<< " q=" << brick.getQAsVector(state)
<< " u=" << brick.getUAsVector(state)
<< endl;
cout << "\nChoose 'Go' from Run menu to simulate:\n";
int menuId, item;
do { silo->waitForMenuPick(menuId, item);
if (menuId != RunMenuId || item != GoItem)
cout << "\aDude ... follow instructions!\n";
} while (menuId != RunMenuId || item != GoItem);
// Simulate it.
// The system as parameterized is very stiff (mostly due to friction)
// and thus runs best with CPodes which is extremely stable for
// stiff problems. To get reasonable performance out of the explicit
// integrators (like the RKs) you'll have to run at a very loose
// accuracy like 0.1, or reduce the friction coefficients and
// maybe the stiffnesses.
//SemiExplicitEuler2Integrator integ(system);
//CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
//VerletIntegrator integ(system);
//integ.setMaximumStepSize(1e-0001);
//integ.setAccuracy(1e-3); // minimum for CPodes
integ.setAccuracy(1e-5);
//integ.setAccuracy(.01);
TimeStepper ts(system, integ);
ts.initialize(state);
double cpuStart = cpuTime();
double realStart = realTime();
ts.stepTo(20.0);
const double timeInSec = realTime() - realStart;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
<< (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*ts.getTime())/evals << "ms/eval\n";
cout << " CPU time was " << cpuTime() - cpuStart << "s\n";
printf("Using Integrator %s at accuracy %g:\n",
integ.getMethodName(), integ.getAccuracyInUse());
printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
viz.dumpStats(std::cout);
// Add as slider to control playback speed.
viz.addSlider("Speed", 1, 0, 4, 1);
viz.setMode(Visualizer::PassThrough);
silo->clear(); // forget earlier input
double speed = 1; // will change if slider moves
while(true) {
cout << "Choose Run/Replay to see that again ...\n";
int menuId, item;
silo->waitForMenuPick(menuId, item);
if (menuId != RunMenuId) {
cout << "\aUse the Run menu!\n";
continue;
}
if (item == QuitItem)
break;
if (item != ReplayItem) {
cout << "\aHuh? Try again.\n";
continue;
}
for (double i=0; i < (int)saveEm.size(); i += speed ) {
int slider; Real newValue;
if (silo->takeSliderMove(slider,newValue)) {
speed = newValue;
}
viz.report(saveEm[(int)i]);
}
}
} catch (const std::exception& e) {
std::printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
} catch (...) {
std::printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
return 0;
}
void generateDecorations(const State& state,
Array_<DecorativeGeometry>& geometry) override
{
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);
DecorativeText energy; energy.setIsScreenText(true);
energy.setText("Energy/KE: " + String(E, "%.6f") + String(KE, "/%.6f"));
geometry.push_back(energy);
//cout << "brick q=" << m_brick.getQAsVector(state) << endl;
//cout << "brick u=" << m_brick.getUAsVector(state) << endl;
m_mbs.realize(state, Stage::Acceleration);
for (unsigned i=0; i < m_balls.size(); ++i) {
const Vec3 f_GC = m_balls[i].findForceOnSphereInG(state);
const Vec3 p_GC = m_balls[i].findContactPointInG(state);
geometry.push_back(
DecorativeLine(p_GC - f_GC, p_GC).setColor(Red));
DecorativeText sep; sep.setIsScreenText(true);
sep.setText(String(i) + ": " +
String(m_balls[i].findSeparation(state), "%.6f"));
geometry.push_back(sep);
sep.setText(" : " +
String(m_balls[i].getVelocityErrors(state)));
geometry.push_back(sep);
sep.setText(" : " +
String(m_balls[i].getAccelerationErrors(state)));
geometry.push_back(sep);
}
for (unsigned i=0; i < m_sphsph.size(); ++i) {
const Vec3 f_GC = m_sphsph[i].findForceOnSphereBInG(state);
const Transform X_GC = m_sphsph[i].findContactFrameInG(state);
geometry.push_back(
DecorativeFrame().setTransform(X_GC).setColor(Purple));
geometry.push_back(
DecorativeLine(X_GC.p() - f_GC, X_GC.p()).setColor(Red));
DecorativeText sep; sep.setIsScreenText(true);
sep.setText(String(i) + ": " +
String(m_sphsph[i].findSeparation(state), "%.6f"));
geometry.push_back(sep);
sep.setText(" : " +
String(m_sphsph[i].getVelocityErrors(state)));
geometry.push_back(sep);
sep.setText(" : " +
String(m_sphsph[i].getAccelerationErrors(state)));
geometry.push_back(sep);
//const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
//State s2 = state;
//m_mbs.realize(s2,Stage::Acceleration);
//Vector udot=s2.getUDot();
//Vec3 aerr(0);
//const Real du = 1e-6;
//for (int u=0; u < matter.getNumMobilities(); ++u) {
// s2.updU()[u] += du; m_mbs.realize(s2,Stage::Velocity);
// Vec3 verrp = m_sphsph[i].getVelocityErrors(s2);
// s2.updU()[u] -= 2*du; m_mbs.realize(s2,Stage::Velocity);
// Vec3 verrm = m_sphsph[i].getVelocityErrors(s2);
// aerr += ((verrp-verrm) / (2*du)) * udot[u];
// s2.updU()[u]=state.getU()[u];
//}
//printf("aerr =%.15g %.15g\n", aerr[0], aerr[1]);
//m_mbs.realize(s2, Stage::Acceleration);
}
for (unsigned i=0; i < m_rods.size(); ++i) {
const Constraint::Rod& rod = m_rods[i];
const Real t = rod.getRodTension(state);
const UnitVec3 d = rod.findRodOrientationInG(state); // p1->p2
const Vec3 f1_G = t*d; // force on p1
const Vec3 f2_G = -f1_G; // force on p2
const Vec3 p2 = rod.getPointOnBody2(state);
const Vec3 p2_G = rod.getMobilizedBody2().
findStationLocationInGround(state, p2);
geometry.push_back(
DecorativeLine(p2_G - f2_G, p2_G)
.setColor(Red).setLineThickness(5));
}
}
int main() {
// Define the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.8/10);
//Force::GlobalDamper damp(forces, matter, 1);
// Describe mass and visualization properties for a generic body.
Real mass = 2;
Vec3 hdim(1,.5,.25);
Body::Rigid bodyInfo(MassProperties(mass, Vec3(0), UnitInertia::brick(hdim)));
bodyInfo.addDecoration(Transform(),
DecorativeBrick(hdim).setColor(Orange).setOpacity(.3));
Real pmass = .1;
Vec3 phdim(5,.5,2);
Body::Rigid platformBody(MassProperties(10*mass,
Vec3(0), UnitInertia::ellipsoid(phdim)));
platformBody.addDecoration(Transform(),
DecorativeEllipsoid(phdim).setColor(Cyan).setOpacity(.1)
.setResolution(5));
MobilizedBody::Ball platform(matter.Ground(), Vec3(0),
platformBody, phdim/2);
//MobilizedBody platform = matter.Ground();
// Create the moving (mobilized) bodies of the pendulum.
//MobilizedBody::Free brick(platform, Transform(Vec3(0)),
// bodyInfo, Transform(Vec3(0)));
MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0)),
bodyInfo, Transform(Vec3(0)));
Array_<Constraint::SphereOnPlaneContact> balls;
Array_<Constraint::SphereOnSphereContact> sphsph;
Array_<Constraint::Rod> rods;
Rotation ZtoY(-Pi/2, XAxis);
//Constraint::PointInPlaneWithStiction pt1(platform,
// Transform(ZtoY, Vec3(0,1,0)),
// brick, hdim);
//pt1.setPlaneDisplayHalfWidth(5);
//Constraint::SphereOnPlaneContact ball1(platform,
// Transform(ZtoY, Vec3(0,1,0)),
// brick, hdim, 0.5, false);
//ball1.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball1);
//Constraint::SphereOnPlaneContact ball2(brick,
// Transform(Vec3(0,0,-hdim[2])),
// platform, -phdim/2, 0.5, false);
//ball2.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball2);
//Constraint::SphereOnPlaneContact ball3(brick,
// Transform(Vec3(0,0,-hdim[2])),
// platform, Vec3(-2,3,-.5), .7, true);
//ball3.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball3);
//MobilizedBody::Free ball(matter.Ground(), Vec3(0),
// MassProperties(1,Vec3(0),UnitInertia(1,1,1)),
// Vec3(0));
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,1,-.5), .7,
// ball, Vec3(0), 1.2, true);
//Constraint::SphereOnSphereContact bb(brick, hdim, 0.5,
// ball, Vec3(0), 1.2, true);
//sphsph.push_back(bb);
Constraint::SphereOnSphereContact ss(brick, hdim, 0.5,
platform, Vec3(-3,1,-.5), 1.2,
false);
sphsph.push_back(ss);
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7,
// brick, hdim, 0.5, false);
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7,
// brick, hdim, 0.5, false);
//Constraint::SphereOnSphereContact ss(brick, hdim, 0.5,
// matter.Ground(), Vec3(-2,3,-.5), .7,true);
Constraint::Rod rod1(brick, Vec3(0,hdim[1],hdim[2]),
platform, Vec3(0,3,-.5), 1.5*1.2);
// Spring to keep the brick near 000.
//Force::TwoPointLinearSpring(forces, platform, Vec3(0),
// brick, Vec3(0), 4, 1);
// Rod to keep the brick near 000.
//Constraint::Rod rod1(platform, Vec3(0,0,2),
// brick, -hdim, 3);
//rods.push_back(rod1);
// Try edge/edge contact.
Constraint::LineOnLineContact ll(platform,
Transform(Rotation(UnitVec3(1,1,1), XAxis, UnitVec3(-XAxis), ZAxis),
Vec3(1,1,1)),
2, // hlen
brick,
Transform(Rotation(UnitVec3(ZAxis), XAxis, Vec3(-1,-1,0), ZAxis),
Vec3(-hdim[0],-hdim[1],0)),
2, // hlen
true);
// Set up visualization at 30 fps.
Visualizer viz(system);
viz.setBackgroundType(Visualizer::SolidColor);
viz.setShowFrameRate(true);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Initialize the system and acquire default state.
State state = system.realizeTopology();
brick.setQToFitTransform(state, Vec3(0,5,0));
brick.setUToFitAngularVelocity(state, Vec3(10,10,10));
//rod1.setRodLength(state, 5);
viz.report(state);
printf("Initial config. Ready to assemble.\n"); getchar();
Assembler asmb(system);
asmb.assemble(state);
viz.report(state);
printf("Assembled. Ready to initialize.\n"); getchar();
//printf("Changed ball3 from rad=%g to rad=%g\n",
// ball3.getSphereRadius(state), 1.5);
//ball3.setSphereRadius(state, 1.5);
//viz.report(state); getchar();
//asmb.assemble(state);
//viz.report(state);
//printf("Re-assembled. Ready to simulate.\n"); getchar();
// Choose integrator and simulate for 10 seconds.
RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
integ.setAccuracy(1e-8);
//integ.setConstraintTolerance(1e-3);
TimeStepper ts(system, integ);
ts.initialize(state);
viz.report(ts.getState());
printf("Initialized. Ready to simulate.\n"); getchar();
viz.addDecorationGenerator(new ShowEnergy(system,brick,balls,sphsph,rods));
ts.stepTo(100.0);
printf("# steps=%d/%d\n",
integ.getNumStepsTaken(), integ.getNumStepsAttempted());
}
// We assume a path name structure like this:
// (1) Everything up to and including the final directory separator
// character is the directory; the rest is the file name. On return
// we fix the slashes in the directory name to suit the current
// system ('\' for Windows, '/' otherwise).
// (2) If the file name contains a ".", characters after the last
// "." are the extension and the last "." is removed.
// (3) What's left is the fileName.
// We accept both "/" and "\" as separator characters. We leave the
// case as it was supplied.
// Leading and trailing white space is removed; embedded white space
// remains.
// Leading "X:" for some drive letter X is recognized on Windows as
// a drive specification, otherwise the drive is the current drive.
// That is then removed for further processing.
// Absolute paths are designated like this:
// Leading "/" means root relative (on the drive).
// Leading "./" means current working directory relative (on the drive).
// Leading "../" is interpreted as "./..".
// Leading "@/" means relative to executable location.
// Above leading characters are removed and replaced with the
// full path name they represent, then the entire path is divided
// into components. If a component is "." or "" (empty) it is
// removed. If a component is ".." it and the previous component
// if any are removed. (".." as a first component will report as
// ill formed.)
// If there is something ill-formed about the file name we'll return
// false.
void Pathname::deconstructPathname( const string& pathname,
bool& dontApplySearchPath,
string& directory,
string& fileName,
string& extension)
{
dontApplySearchPath = false;
directory.erase(); fileName.erase(); extension.erase();
// Remove all the white space and make all the slashes be forward ones.
// (For Windows they'll be changed to backslashes later.)
String processed = String::trimWhiteSpace(pathname)
.replaceAllChar('\\', '/');
if (processed.empty())
return; // pathname consisted only of white space
string drive;
removeDriveInPlace(processed, drive);
// Now the drive if any has been removed and we're looking at
// the beginning of the pathname.
// If the pathname in its entirety is just one of these, append
// a slash to avoid special cases below.
if (processed == "." || processed == ".." || processed == "@")
processed += "/";
// If the path begins with "../" we'll make it ./../ to simplify handling.
if (processed.substr(0, 3) == "../")
processed.insert(0, "./");
if (processed.substr(0, 1) == "/") {
dontApplySearchPath = true;
processed.erase(0, 1);
if (drive.empty()) drive = getCurrentDriveLetter();
}
else if (processed.substr(0, 2) == "./") {
dontApplySearchPath = true;
processed.replace(0, 2, getCurrentWorkingDirectory(drive));
removeDriveInPlace(processed, drive);
}
else if (processed.substr(0, 2) == "@/") {
dontApplySearchPath = true;
processed.replace(0, 2, getThisExecutableDirectory());
removeDriveInPlace(processed, drive);
}
else if (!drive.empty()) {
// Looks like a relative pathname. But if it had an initial
// drive specification, e.g. X:something.txt, that is supposed
// to be interpreted relative to the current working directory
// on drive X, just as though it were X:./something.txt.
dontApplySearchPath = true;
processed.insert(0, getCurrentWorkingDirectory(drive));
removeDriveInPlace(processed, drive);
}
// We may have picked up a new batch of backslashes above.
processed.replaceAllChar('\\', '/');
// Now we have the full pathname if this is absolute, otherwise
// we're looking at a relative pathname. In any case the last
// component is the file name if it isn't empty, ".", or "..".
// Process the ".." segments and eliminate meaningless ones
// as we go through.
Array_<string> segmentsInReverse;
bool isFinalSegment = true; // first time around might be the fileName
int numDotDotsSeen = 0;
while (!processed.empty()) {
string component;
removeLastPathComponentInPlace(processed, component);
if (component == "..")
++numDotDotsSeen;
else if (!component.empty() && component != ".") {
if (numDotDotsSeen)
--numDotDotsSeen; // skip component
else if (isFinalSegment) fileName = component;
else segmentsInReverse.push_back(component);
}
isFinalSegment = false;
}
// Now we can put together the canonicalized directory.
if (dontApplySearchPath) {
if (!drive.empty())
directory = drive + ":";
directory += "/";
}
for (int i = (int)segmentsInReverse.size() - 1; i >= 0; --i)
directory += segmentsInReverse[i] + "/";
// Fix the slashes.
makeNativeSlashesInPlace(directory);
// If there is a .extension, strip it off.
string::size_type lastDot = fileName.rfind('.');
if (lastDot != string::npos) {
extension = fileName.substr(lastDot);
fileName.erase(lastDot);
}
}
//------------------------- PROCESS EXPANSION PHASE ----------------------------
bool ContactOn::
processExpansionPhase(MyElementSubset& proximal,
State& s) const
{
SimTK_DEBUG("Entering processExpansionPhase() ...\n");
// Generate an expansion impulse if there were any active contacts that
// still have some restitution remaining.
Vector expansionImpulse;
bool anyChange = false;
for (unsigned i=0; i<proximal.m_contact.size(); ++i) {
const int which = proximal.m_contact[i];
MyContactElement& uni = m_unis.updContactElement(which);
if (uni.isDisabled(s)||uni.isRestitutionDone()
||uni.getEffectiveCoefRest()==0
||uni.getCompressionImpulse()<=0)
continue;
uni.setMyExpansionImpulse(s, uni.getEffectiveCoefRest(),
expansionImpulse);
uni.recordImpulse(MyContactElement::Expansion,s,expansionImpulse);
uni.setRestitutionDone(true);
anyChange = true;
}
if (!anyChange) {
SimTK_DEBUG("... no expansion impulse -- done.\n");
return false;
}
// We generated an expansion impulse. Apply it and update velocities.
updateVelocities(Vector(), expansionImpulse, s);
// Release any constraint that now has a positive velocity.
Array_<int> toDisable;
for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
const int which = proximal.m_contact[i];
const MyContactElement& uni = m_unis.getContactElement(which);
if (!uni.isDisabled(s) && uni.getVerr(s) > 0)
toDisable.push_back(which);
}
// Now do the actual disabling (can't mix this with checking velocities)
// because disabling invalidates Instance stage.
for (unsigned i=0; i < toDisable.size(); ++i) {
const int which = toDisable[i];
const MyContactElement& uni = m_unis.getContactElement(which);
uni.disable(s);
}
SimTK_DEBUG(" Expansion results:\n");
m_mbs.realize(s, Stage::Velocity);
for (unsigned i=0; i < proximal.m_contact.size(); ++i) {
const int which = proximal.m_contact[i];
const MyContactElement& uni = m_unis.getContactElement(which);
SimTK_DEBUG4(" %d %3s: Ie=%g, V=%g\n",
which, uni.isDisabled(s) ? "off" : "ON",
uni.getExpansionImpulse(), uni.getVerr(s));
}
SimTK_DEBUG("... expansion phase done.\n");
return true;
}
void DefaultGeometry::generateDecorations
(const State& state,
Array_<SimTK::DecorativeGeometry>& geometry)
{
const SimbodyMatterSubsystem& matter = _model.getMatterSubsystem();
const ModelDisplayHints& hints = _model.getDisplayHints();
// Display wrap objects.
if (hints.get_show_wrap_geometry()) {
const Vec3 color(SimTK::Cyan);
Transform ztoy;
ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
const BodySet& bodies = _model.getBodySet();
for (int i = 0; i < bodies.getSize(); i++) {
const OpenSim::Body& body = bodies[i];
const Transform& X_GB =
body.getMobilizedBody().getBodyTransform(state);
const WrapObjectSet& wrapObjects = body.getWrapObjectSet();
for (int j = 0; j < wrapObjects.getSize(); j++) {
const string type = wrapObjects[j].getConcreteClassName();
if (type == "WrapCylinder") {
const WrapCylinder* cylinder =
dynamic_cast<const WrapCylinder*>(&wrapObjects[j]);
if (cylinder != NULL) {
Transform X_GW = X_GB*cylinder->getTransform()*ztoy;
geometry.push_back(
DecorativeCylinder(cylinder->getRadius(),
cylinder->getLength()/2)
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
else if (type == "WrapEllipsoid") {
const WrapEllipsoid* ellipsoid =
dynamic_cast<const WrapEllipsoid*>(&wrapObjects[j]);
if (ellipsoid != NULL) {
Transform X_GW = X_GB*ellipsoid->getTransform();
geometry.push_back(
DecorativeEllipsoid(ellipsoid->getRadii())
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
else if (type == "WrapSphere") {
const WrapSphere* sphere =
dynamic_cast<const WrapSphere*>(&wrapObjects[j]);
if (sphere != NULL) {
Transform X_GW = X_GB*sphere->getTransform();
geometry.push_back(
DecorativeSphere(sphere->getRadius())
.setTransform(X_GW).setResolution(_dispWrapResolution)
.setColor(color).setOpacity(_dispWrapOpacity));
}
}
}
}
}
// Display contact geometry objects.
if (hints.get_show_contact_geometry()) {
const Vec3 color(SimTK::Green);
Transform ztoy;
ztoy.updR().setRotationFromAngleAboutX(SimTK_PI/2);
const ContactGeometrySet& contactGeometries = _model.getContactGeometrySet();
for (int i = 0; i < contactGeometries.getSize(); i++) {
const PhysicalFrame& body = contactGeometries.get(i).getBody();
const Transform& X_GB =
matter.getMobilizedBody(body.getMobilizedBodyIndex()).getBodyTransform(state);
const string type = contactGeometries.get(i).getConcreteClassName();
const int displayPref = contactGeometries.get(i).getDisplayPreference();
//cout << type << ": " << contactGeometries.get(i).getName() << ": disp pref = " << displayPref << endl;
if (type == "ContactSphere" && displayPref == 4) {
ContactSphere* sphere =
dynamic_cast<ContactSphere*>(&contactGeometries.get(i));
if (sphere != NULL) {
Transform X_GW = X_GB*sphere->getTransform();
geometry.push_back(
DecorativeSphere(sphere->getRadius())
.setTransform(X_GW).setResolution(_dispContactResolution)
.setColor(color).setOpacity(_dispContactOpacity));
}
}
}
}
// Ask all the ModelComponents to generate dynamic geometry.
_model.generateDecorations(false, _model.getDisplayHints(),
state, geometry);
}
