本文整理汇总了C++中MultibodySystem::getRigidBodyForces方法的典型用法代码示例。如果您正苦于以下问题：C++ MultibodySystem::getRigidBodyForces方法的具体用法？C++ MultibodySystem::getRigidBodyForces怎么用？C++ MultibodySystem::getRigidBodyForces使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MultibodySystem的用法示例。
在下文中一共展示了MultibodySystem::getRigidBodyForces方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: testForces
void testForces() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralContactSubsystem contacts(system);
GeneralForceSubsystem forces(system);
// Create a triangle mesh in the shape of a pyramid, with the
// square base having area 1 (split into two triangles).
vector<Vec3> vertices;
vertices.push_back(Vec3(0, 0, 0));
vertices.push_back(Vec3(1, 0, 0));
vertices.push_back(Vec3(1, 0, 1));
vertices.push_back(Vec3(0, 0, 1));
vertices.push_back(Vec3(0.5, 1, 0.5));
vector<int> faceIndices;
int faces[6][3] = {{0, 1, 2}, {0, 2, 3}, {1, 0, 4},
{2, 1, 4}, {3, 2, 4}, {0, 3, 4}};
for (int i = 0; i < 6; i++)
for (int j = 0; j < 3; j++)
faceIndices.push_back(faces[i][j]);
// Create the mobilized bodies and configure the contact model.
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
ContactSetIndex setIndex = contacts.createContactSet();
MobilizedBody::Translation mesh(matter.updGround(), Transform(), body, Transform());
contacts.addBody(setIndex, mesh, ContactGeometry::TriangleMesh(vertices, faceIndices), Transform());
contacts.addBody(setIndex, matter.updGround(), ContactGeometry::HalfSpace(), Transform(Rotation(-0.5*Pi, ZAxis), Vec3(0))); // y < 0
ElasticFoundationForce ef(forces, contacts, setIndex);
Real stiffness = 1e9, dissipation = 0.01, us = 0.1, ud = 0.05, uv = 0.01, vt = 0.01;
ef.setBodyParameters(ContactSurfaceIndex(0), stiffness, dissipation, us, ud, uv);
ef.setTransitionVelocity(vt);
ASSERT(ef.getTransitionVelocity() == vt);
State state = system.realizeTopology();
// Position the pyramid at a variety of positions and check the normal
// force.
for (Real depth = -0.1; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
system.realize(state, Stage::Dynamics);
Real f = 0;
if (depth > 0)
f = stiffness*depth;
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[matter.getGround().getMobilizedBodyIndex()][1], Vec3(0, -f, 0));
}
// Now do it with a vertical velocity and see if the dissipation force is correct.
for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
for (Real v = -1.0; v <= 1.0; v += 0.1) {
mesh.setUToFitLinearVelocity(state, Vec3(0, -v, 0));
system.realize(state, Stage::Dynamics);
Real f = (depth > 0 ? stiffness*depth*(1+dissipation*v) : 0);
if (f < 0)
f = 0;
assertEqual(system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()][1], Vec3(0, f, 0));
}
}
// Do it with a horizontal velocity and see if the friction force is correct.
Vector_<SpatialVec> expectedForce(matter.getNumBodies());
for (Real depth = -0.105; depth < 0.1; depth += 0.01) {
mesh.setQToFitTranslation(state, Vec3(0, -depth, 0));
Real fh = 0;
if (depth > 0)
fh = stiffness*depth;
for (Real v = -1.0; v <= 1.0; v += 0.1) {
mesh.setUToFitLinearVelocity(state, Vec3(v, 0, 0));
system.realize(state, Stage::Dynamics);
const Real vrel = std::abs(v/vt);
Real ff = (v < 0 ? 1 : -1)*fh*(std::min(vrel, 1.0)*(ud+2*(us-ud)/(1+vrel*vrel))+uv*std::fabs(v));
const Vec3 totalForce = Vec3(ff, fh, 0);
expectedForce = SpatialVec(Vec3(0), Vec3(0));
Vec3 contactPoint1 = mesh.findStationAtGroundPoint(state, Vec3(2.0/3.0, 0, 1.0/3.0));
mesh.applyForceToBodyPoint(state, contactPoint1, 0.5*totalForce, expectedForce);
Vec3 contactPoint2 = mesh.findStationAtGroundPoint(state, Vec3(1.0/3.0, 0, 2.0/3.0));
mesh.applyForceToBodyPoint(state, contactPoint2, 0.5*totalForce, expectedForce);
SpatialVec actualForce = system.getRigidBodyForces(state, Stage::Dynamics)[mesh.getMobilizedBodyIndex()];
assertEqual(actualForce[0], expectedForce[mesh.getMobilizedBodyIndex()][0]);
assertEqual(actualForce[1], expectedForce[mesh.getMobilizedBodyIndex()][1]);
}
}
}