void ElasticFoundationForceImpl::calcForce
(const State& state, Vector_<SpatialVec>& bodyForces,
Vector_<Vec3>& particleForces, Vector& mobilityForces) const
{
const Array_<Contact>& contacts = subsystem.getContacts(state, set);
Real& pe = Value<Real>::downcast
(subsystem.updCacheEntry(state, energyCacheIndex));
pe = 0.0;
for (int i = 0; i < (int) contacts.size(); i++) {
std::map<ContactSurfaceIndex, Parameters>::const_iterator iter1 =
parameters.find(contacts[i].getSurface1());
std::map<ContactSurfaceIndex, Parameters>::const_iterator iter2 =
parameters.find(contacts[i].getSurface2());
// If there are two meshes, scale each one's contributions by 50%.
Real areaScale = (iter1==parameters.end() || iter2==parameters.end())
? Real(1) : Real(0.5);
if (iter1 != parameters.end()) {
const TriangleMeshContact& contact =
static_cast<const TriangleMeshContact&>(contacts[i]);
processContact(state, contact.getSurface1(),
contact.getSurface2(), iter1->second,
contact.getSurface1Faces(), areaScale, bodyForces, pe);
}
if (iter2 != parameters.end()) {
const TriangleMeshContact& contact =
static_cast<const TriangleMeshContact&>(contacts[i]);
processContact(state, contact.getSurface2(),
contact.getSurface1(), iter2->second,
contact.getSurface2Faces(), areaScale, bodyForces, pe);
}
}
}
int HuntCrossleyContactRep::realizeSubsystemDynamicsImpl(const State& s) const
{
const Parameters& p = getParameters(s);
if (!p.enabled) return 0;
const MultibodySystem& mbs = getMultibodySystem(); // my owner
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
Real& pe = Value<Real>::updDowncast(s.updCacheEntry(getMySubsystemIndex(), energyCacheIndex)).upd();
pe = 0;
// Get access to system-global cache entries.
Vector_<SpatialVec>& rigidBodyForces = mbs.updRigidBodyForces(s, Stage::Dynamics);
for (int s1=0; s1 < (int)p.spheres.size(); ++s1) {
const SphereParameters& sphere1 = p.spheres[s1];
const Transform& X_GB1 = matter.getMobilizedBody(sphere1.body).getBodyTransform(s);
const SpatialVec& V_GB1 = matter.getMobilizedBody(sphere1.body).getBodyVelocity(s); // in G
const Real r1 = sphere1.radius;
const Vec3 center1_G = X_GB1*sphere1.center;
for (int s2=s1+1; s2 < (int)p.spheres.size(); ++s2) {
const SphereParameters& sphere2 = p.spheres[s2];
if (sphere2.body == sphere1.body) continue;
const Transform& X_GB2 = matter.getMobilizedBody(sphere2.body).getBodyTransform(s);
const SpatialVec& V_GB2 = matter.getMobilizedBody(sphere2.body).getBodyVelocity(s);
const Real r2 = sphere2.radius;
const Vec3 center2_G = X_GB2*sphere2.center; // 18 flops
const Vec3 c2c1_G = center1_G - center2_G; // points at sphere1 (3 flops)
const Real dsq = c2c1_G.normSqr(); // 5 flops
if (dsq >= (r1+r2)*(r1+r2)) continue; // 4 flops
// There is a collision
const Real d = std::sqrt(dsq); // distance between the centers (~30 flops)
const UnitVec3 normal_G(c2c1_G/d, true); // direction from c2 center to c1 center (~12 flops)
processContact((r1*r2)/(r1+r2), // relative curvature, ~12 flops
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
sphere2.stiffness, sphere2.dissipation, X_GB2, V_GB2,
center1_G - r1 * normal_G, // undeformed contact point for sphere1 (6 flops)
center2_G + r2 * normal_G, // undeformed contact point for sphere2 (6 flops)
normal_G,
pe, rigidBodyForces[sphere1.body], rigidBodyForces[sphere2.body]);
}
// half spaces
for (int h=0; h < (int)p.halfSpaces.size(); ++h) {
const HalfspaceParameters& halfSpace = p.halfSpaces[h];
if (halfSpace.body == sphere1.body) continue;
// Quick escape in the common case where the half space is on ground (7 flops)
if (halfSpace.body==0) {
const UnitVec3& normal_G = halfSpace.normal;
const Real hc1_G = ~center1_G*normal_G; // ht of center over ground
if (hc1_G - halfSpace.height < r1) {
// Collision of sphere1 with a half space on ground.
processContact(r1, // relative curvature is just r1
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
halfSpace.stiffness, halfSpace.dissipation, Transform(), SpatialVec(Vec3(0)),
center1_G - (r1*normal_G), // undeformed contact point for sphere (6 flops)
halfSpace.height*normal_G, // " for halfSpace (3 flops)
normal_G, pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
}
continue;
}
// Half space is not on ground.
const Transform& X_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyTransform(s);
const SpatialVec& V_GB2 = matter.getMobilizedBody(halfSpace.body).getBodyVelocity(s);
const UnitVec3 normal_G = X_GB2.R()*halfSpace.normal; // 15 flops
// Find the heights of the half space surface and sphere center measured
// along the contact normal from the ground origin. Then we can get the
// height of the sphere center over the half space surface.
const Real h_G = ~X_GB2.p()*normal_G + halfSpace.height; // 6 flops
const Real hc1_G = ~center1_G*normal_G; // 5 flops
const Real d = hc1_G - h_G; // 1 flop
if (d >= r1) continue; // 1 flop
// There is a collision
processContact(r1, // relative curvature is just r1
sphere1.stiffness, sphere1.dissipation, X_GB1, V_GB1,
halfSpace.stiffness, halfSpace.dissipation, X_GB2, V_GB2,
center1_G - r1 * normal_G, // undeformed contact point for sphere (6 flops)
center1_G - d * normal_G, // undeformed contact point for halfSpace (6 flops)
normal_G,
pe, rigidBodyForces[sphere1.body], rigidBodyForces[halfSpace.body]);
}
}
return 0;
}