C++ Array_::push_back方法代码示例

//.........这里部分代码省略.........

				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);
//.........这里部分代码省略.........

// 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
//.........这里部分代码省略.........

 void handleEvent(const State& state) const {
     saveEm.push_back(state);
 }

//.........这里部分代码省略.........
        }
    }

    // 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;

    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)),
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                                    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;
}