C++ Body::updDisplayer方法代码示例

int main(int argc, char **argv) {
  cout << "--------------------------------------------------------------------------------" << endl;
  cout << " Multi-Body System Benchmark in OpenSim" << endl;
  cout << " Benchmark reference url: http://lim.ii.udc.es/mbsbenchmark/" << endl;
  cout << " Problem A03: Andrew's Mechanism Model Creator" << endl;
  cout << " Copyright (C) 2013-2015 Luca Tagliapietra, Michele Vivian, Elena Ceseracciu, and Monica Reggiani" << endl;
  cout << "--------------------------------------------------------------------------------" << endl;

  if (argc != 2){
    cout << " ******************************************************************************" << endl;
    cout << " Multi-Body System Benchmark in OpenSim: Creator for Model A03" << endl;
    cout << " Usage: ./AndrewsMechanismCreateModel dataDirectory" << endl;
    cout << "       dataDirectory must contain a vtpFiles folder" << endl;
    cout << " ******************************************************************************" << endl;
    exit(EXIT_FAILURE);
  }

  const std::string dataDir = argv[1];
  cout << "Data directory: " + dataDir << endl;

  OpenSim::Model andrewsMechanism;
  andrewsMechanism.setName("Andrew's Mechanism");
  andrewsMechanism.setAuthors("L.Tagliapietra, M. Vivian, M.Reggiani");

  // Get a reference to the model's ground body
  OpenSim::Body& ground = andrewsMechanism.getGroundBody();
  andrewsMechanism.setGravity(gravityVector);

  //******************************
  // Create OF element
  //******************************
  SimTK::Inertia OFbarInertia(0.1,0.1,OFinertia);
  OpenSim::Body *OF = new OpenSim::Body("OF", OFmass, OFMassCenter, OFbarInertia);

  //Set transformation for visualization pourpose
  SimTK::Rotation rot(SimTK::Pi/2, SimTK::UnitVec3(0,0,1));
  SimTK::Transform trans = SimTK::Transform(rot);

  //Set visualization properties
  OF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visOF = OF->updDisplayer();
  visOF -> updTransform() =  trans;
  visOF -> setScaleFactors(SimTK::Vec3(0.001,OFlength, 0.001));
  visOF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  SimTK::Vec3 orientationInParent(0), orientationInBody(0);
  OpenSim::PinJoint *OJoint = new OpenSim::PinJoint("joint_O", ground, SimTK::Vec3(0), orientationInParent, *OF, SimTK::Vec3(-OFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& OCoordinateSet = OJoint -> upd_CoordinateSet();
  OCoordinateSet[0].setName("joint_O");
  OCoordinateSet[0].setDefaultValue(OAngleAtZero);
  andrewsMechanism.addBody(OF);

  //********************************
  // Create FE element
  //********************************
  SimTK::Inertia EFbarInertia(0.1,0.1,EFinertia);
  OpenSim::Body *EF = new OpenSim::Body("EF", EFmass, EFMassCenter, EFbarInertia);

  //Set visualization properties
  EF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visEF = EF->updDisplayer();
  visEF -> updTransform() =  trans;
  visEF -> setScaleFactors(SimTK::Vec3(0.001,EFlength, 0.001));
  visEF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *FJoint = new OpenSim::PinJoint("joint_F", *OF, SimTK::Vec3(OFlength/2,0,0), orientationInParent, *EF, SimTK::Vec3(EFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& FCoordinateSet = FJoint -> upd_CoordinateSet();
  FCoordinateSet[0].setName("joint_F");
  FCoordinateSet[0].setDefaultValue(FAngleAtZero);
  andrewsMechanism.addBody(EF);

  //********************************
  // Create EG element
  //********************************
  SimTK::Inertia GEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *GE = new OpenSim::Body("GE", GEmass, GEMassCenter, GEbarInertia);

  //Set visualization properties
  GE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visGE = GE->updDisplayer();
  visGE -> updTransform() =  trans;
  visGE -> setScaleFactors(SimTK::Vec3(0.001,GElength, 0.001));
  visGE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E1Joint = new OpenSim::PinJoint("joint_E1", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *GE, SimTK::Vec3(GElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E1CoordinateSet = E1Joint -> upd_CoordinateSet();
  E1CoordinateSet[0].setName("joint_E1");
  E1CoordinateSet[0].setDefaultValue(E1AngleAtZero);
  andrewsMechanism.addBody(GE);

  //********************************
  // Create AG element
  //********************************
  SimTK::Inertia AGbarInertia(0.1,0.1,AGinertia);
  OpenSim::Body *AG = new OpenSim::Body("AG", AGmass, AGMassCenter, AGbarInertia);

  //Set visualization properties
  AG->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAG = AG->updDisplayer();
  visAG -> updTransform() =  trans;
//.........这里部分代码省略.........

/**
 * Run a simulation of block sliding with contact on by two muscles sliding with contact 
 */
int main()
{

	try {
		// Create a new OpenSim model
		Model osimModel;
		osimModel.setName("osimModel");

		double Pi = SimTK::Pi;
		
		
		// Get the ground body
		OpenSim::Body& ground = osimModel.getGroundBody();
		ground.addDisplayGeometry("checkered_floor.vtp");

		// create linkage body
		double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
		
		Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
		Vec3 linkageMassCenter(0,linkageLength/2,0);
		Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);

		OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);

		// Graphical representation
		linkage1->addDisplayGeometry("cylinder.vtp");
		//This cylinder.vtp geometry is 1 meter tall, 1 meter diameter.  Scale and shift it to look pretty
		GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet();
		DisplayGeometry& thinCylinder = geometry[0];
		thinCylinder.setScaleFactors(linkageDimensions);
		thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0)));
		linkage1->addDisplayGeometry("sphere.vtp");
		//This sphere.vtp is 1 meter in diameter.  Scale it.
		geometry[1].setScaleFactors(Vec3(0.1));
		
		// Creat a second linkage body
		OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
		linkage2->setName("linkage2");

		// Creat a block to be the pelvis
		double blockMass = 20.0, blockSideLength = 0.2;
		Vec3 blockMassCenter(0);
		Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
		OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
		block->addDisplayGeometry("block.vtp");
		//This block.vtp is 0.1x0.1x0.1 meters.  scale its appearance
		block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0));

		// Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
		
		Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);

		PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, 
			locationInChild, orientationInChild);

		PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
			locationInChild, orientationInChild);

		PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
			locationInChild, orientationInChild);
		
		double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
		CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
		ankleCoordinateSet[0].setName("q1");
		ankleCoordinateSet[0].setRange(range);

		CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
		kneeCoordinateSet[0].setName("q2");
		kneeCoordinateSet[0].setRange(range);

		CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
		hipCoordinateSet[0].setName("q3");
		hipCoordinateSet[0].setRange(range);

		// Add the bodies to the model
		osimModel.addBody(linkage1);
		osimModel.addBody(linkage2);
		osimModel.addBody(block);

		// Define contraints on the model
		//  Add a point on line constraint to limit the block to vertical motion

		Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
		PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
		osimModel.addConstraint(lineConstraint);

		// Add PistonActuator between the first linkage and the block
		Vec3 pointOnBodies(0);
		PistonActuator *piston = new PistonActuator();
		piston->setName("piston");
		piston->setBodyA(linkage1);
		piston->setBodyB(block);
		piston->setPointA(pointOnBodies);
		piston->setPointB(pointOnBodies);
		piston->setOptimalForce(200.0);
		piston->setPointsAreGlobal(false);

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

/*==============================================================================  
    Main test driver to be used on any muscle model (derived from Muscle) so new 
    cases should be easy to add currently, the test only verifies that the work 
    done by the muscle corresponds to the change in system energy.

    TODO: Test will fail wih prescribe motion until the work done by this 
    constraint is accounted for.
================================================================================
*/
void simulateMuscle(
        const Muscle &aMuscModel, 
        double startX, 
        double act0, 
        const Function *motion,  // prescribe motion of free end of muscle
        const Function *control, // prescribed excitation signal to the muscle
        double integrationAccuracy,
        int testType,
        double testTolerance,
        bool printResults)
{
    string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";

    cout << "\n******************************************************" << endl;
    cout << "Test " << aMuscModel.getConcreteClassName() 
         << " Model" << prescribed << endl;
    cout << "******************************************************" << endl;
    using SimTK::Vec3;

//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================

    // Define the initial and final simulation times
    double initialTime = 0.0;
    double finalTime = 4.0;
    
    //Physical properties of the model
    double ballMass = 10;
    double ballRadius = 0.05;
    double anchorWidth = 0.1;

    // Create an OpenSim model
    Model model;

    double optimalFiberLength = aMuscModel.getOptimalFiberLength();
    double pennationAngle     = aMuscModel.getPennationAngleAtOptimalFiberLength();
    double tendonSlackLength  = aMuscModel.getTendonSlackLength();

    // Use a copy of the muscle model passed in to add path points later
    PathActuator *aMuscle = aMuscModel.clone();

    // Get a reference to the model's ground body
    Body& ground = model.getGroundBody();
    ground.addDisplayGeometry("box.vtp");
    ground.updDisplayer()
        ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));

    OpenSim::Body * ball = new OpenSim::Body("ball", 
                        ballMass , 
                        Vec3(0),  
                        ballMass*SimTK::Inertia::sphere(ballRadius));
    
    ball->addDisplayGeometry("sphere.vtp");
    ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
    // ball connected  to ground via a slider along X
    double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;

    SliderJoint* slider = new SliderJoint( "slider", 
                        ground, 
                        Vec3(anchorWidth/2+xSinG, 0, 0), 
                        Vec3(0), 
                        *ball, 
                        Vec3(0), 
                        Vec3(0));

    CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
        jointCoordinateSet[0].setName("tx");
        jointCoordinateSet[0].setDefaultValue(1.0);
        jointCoordinateSet[0].setRangeMin(0); 
        jointCoordinateSet[0].setRangeMax(1.0);
    
    if(motion != NULL){
        jointCoordinateSet[0].setPrescribedFunction(*motion);
        jointCoordinateSet[0].setDefaultIsPrescribed(true);
    }
    // add ball to model
    model.addBody(ball);
	model.addJoint(slider);


//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================

    //Attach the muscle
    const string &actuatorType = aMuscle->getConcreteClassName();
    aMuscle->setName("muscle");
    aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
    aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));
    
//.........这里部分代码省略.........