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

//==========================================================================================================
// Test Cases
//==========================================================================================================
void testPrescribedForce(OpenSim::Function* forceX, OpenSim::Function* forceY, OpenSim::Function* forceZ,
                 OpenSim::Function* pointX, OpenSim::Function* pointY, OpenSim::Function* pointZ,
                 OpenSim::Function* torqueX, OpenSim::Function* torqueY, OpenSim::Function* torqueZ,
                 vector<SimTK::Real>& times, vector<SimTK::Vec3>& accelerations, vector<SimTK::Vec3>& angularAccelerations)
{
	using namespace SimTK;

	//==========================================================================================================
	// Setup OpenSim model
	Model *osimModel = new Model;
	//OpenSim bodies
    OpenSim::Body& ground = osimModel->getGroundBody();
	OpenSim::Body ball;
	ball.setName("ball");

	// Add joints
	FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0), false);

	// Rename coordinates for a free joint
	CoordinateSet free_coords = free.getCoordinateSet();
	for(int i=0; i<free_coords.getSize(); i++){
		std::stringstream coord_name;
		coord_name << "free_q" << i;
		free_coords.get(i).setName(coord_name.str());
		free_coords.get(i).setMotionType(i > 2 ? Coordinate::Translational : Coordinate::Rotational);
	}

	osimModel->addBody(&ball);
	osimModel->addJoint(&free);

	// Add a PrescribedForce.
	PrescribedForce force(&ball);
    if (forceX != NULL)
        force.setForceFunctions(forceX, forceY, forceZ);
    if (pointX != NULL)
        force.setPointFunctions(pointX, pointY, pointZ);
    if (torqueX != NULL)
        force.setTorqueFunctions(torqueX, torqueY, torqueZ);

	counter++;
	osimModel->updForceSet().append(&force);

	// BAD: have to set memoryOwner to false or program will crash when this test is complete.
	osimModel->disownAllComponents();

    //Set mass
	ball.setMass(ballMass.getMass());
	ball.setMassCenter(ballMass.getMassCenter());
	ball.setInertia(ballMass.getInertia());

	osimModel->setGravity(gravity_vec);
	osimModel->print("TestPrescribedForceModel.osim");

	delete osimModel;
	// Check that serialization/deserialization is working correctly as well
	osimModel = new Model("TestPrescribedForceModel.osim");
    SimTK::State& osim_state = osimModel->initSystem();
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

	//==========================================================================================================
	// Compute the force and torque at the specified times.

	const OpenSim::Body& body = osimModel->getBodySet().get("ball");

    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
    Manager manager(*osimModel,  integrator);
    manager.setInitialTime(0.0);
    for (unsigned int i = 0; i < times.size(); ++i)
    {
        manager.setFinalTime(times[i]);
        manager.integrate(osim_state);
        osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
        Vec3 accel, angularAccel;
        osimModel->updSimbodyEngine().getAcceleration(osim_state, body, Vec3(0), accel);
        osimModel->updSimbodyEngine().getAngularAcceleration(osim_state, body, angularAccel);
        ASSERT_EQUAL(accelerations[i][0], accel[0], 1e-10);
        ASSERT_EQUAL(accelerations[i][1], accel[1], 1e-10);
        ASSERT_EQUAL(accelerations[i][2], accel[2], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][0], angularAccel[0], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][1], angularAccel[1], 1e-10);
        ASSERT_EQUAL(angularAccelerations[i][2], angularAccel[2], 1e-10);
    }
}

// test sphere to sphere contact using elastic foundation with and without 
// meshes and their combination
int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2)
{
    // Setup OpenSim model
    Model *osimModel = new Model;

    //OpenSim bodies
    OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
        Vec3(0), Inertia());
    osimModel->addBody(&ground);

    OpenSim::Body ball;
    ball.setName("ball");
    ball.setMass(mass);
    ball.setMassCenter(Vec3(0));
    ball.setInertia(Inertia(1.0));

    // Add joints
    FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));

    osimModel->addBody(&ball);
    osimModel->addJoint(&free);

    // Create ContactGeometry.
    OpenSim::ContactGeometry *ball1, *ball2;

    if (useElasticFoundation && useMesh1)
        ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1");
    else
        ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1");

    if (useElasticFoundation && useMesh2)
        ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2");
    else
        ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2");
    
    osimModel->addContactGeometry(ball1);
    osimModel->addContactGeometry(ball2);

    OpenSim::Force* force;

    std::string prefix;
    if (useElasticFoundation){
        
    }
    else{
        
    }
    if (useElasticFoundation)
    {
        // Add an ElasticFoundationForce.
        OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball1");
        contactParams->addGeometry("ball2");
        force = new OpenSim::ElasticFoundationForce(contactParams);
        prefix = "EF_";
        prefix += useMesh1 ?"Mesh":"noMesh";
        prefix += useMesh2 ? "_to_Mesh":"_to_noMesh";
        
    }
    else
    {
        // Add a Hertz HuntCrossleyForce.
        OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0);
        contactParams->addGeometry("ball1");
        contactParams->addGeometry("ball2");
        force = new OpenSim::HuntCrossleyForce(contactParams);
        prefix = "Hertz";
        
    }

    force->setName("contact");
    osimModel->addForce(force);
    osimModel->setGravity(gravity_vec);

    osimModel->setName(prefix);
    osimModel->clone()->print(prefix+".osim");

    Kinematics* kin = new Kinematics(osimModel);
    osimModel->addAnalysis(kin);

    ForceReporter* reporter = new ForceReporter(osimModel);
    osimModel->addAnalysis(reporter);

    SimTK::State& osim_state = osimModel->initSystem();
    osim_state.updQ()[4] = height;
    osimModel->getMultibodySystem().realize(osim_state, Stage::Position );

    //==========================================================================================================
    // Simulate it and see if it bounces correctly.
    cout << "stateY=" << osim_state.getY() << std::endl;

    RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
    integrator.setAccuracy(integ_accuracy);
    integrator.setMaximumStepSize(100*integ_accuracy);
    Manager manager(*osimModel, integrator);
    manager.setInitialTime(0.0);
    manager.setFinalTime(duration);
    manager.integrate(osim_state);
//.........这里部分代码省略.........