Geometric representations for contact

Moby supports the following geometric representations for contact between rigid bodies:

• Plane (halfspace, PlanePrimitive)
• Sphere (SpherePrimitive)
• Box (BoxPrimitive)
• Cone (ConePrimitive)
• Cylinder (CylinderPrimitive)
• Torus (TorusPrimitive)
• Heightmap (HeightmapPrimitive)
• Convex polyhedron (PolyhedralPrimitive)

Data sharing

To save memory, Moby allows multiple rigid bodies to use the same geometric shape. Because the pose of a primitive in space can be defined with respect to multiple CollisionGeometry objects, each primitive stores a separate pose relative to each CollisionGeometry it is associated with. The following fragment of code illustrates:

shared_ptr<PolyhedralPrimitive> associate(const std::string& filename, shared_ptr<CollisionGeometry> g1, shared_ptr<CollisionGeometry> g2)
{
  // prepare to rotate the indexed triangle array by 90 degrees around the x-axis
  shared_ptr<Pose3d> rot90(new Pose3d);
  rot90->q = Matrix3d::rot_X(M_PI_2);
  Transform3d T = Pose3d::calc_relative_pose(rot90, GLOBAL);

  // read the *rotated* indexed triangle array from a Wavefront OBJ file
  IndexedTriArray ita = IndexedTriArray::read_from_obj(filename).transform(T);
  
  // polyhedron must be convex for collision detection to work
  const vector<Origin3d>& vertices = ita.get_vertices();
  TessellatedPolyhedronPtr tpoly = CompGeom::calc_convex_hull(vertices.begin(), vertices.end());
  Polyhedron poly;
  tpoly->to_polyhedron(poly);

  // create the primitive
  shared_ptr<PolyhedralPrimitive> P(new PolyhedralPrimitive);
  P->set_polyhedron(poly);

  // associate the primitive with the two collision geometries
  g1->set_geometry(P);
  g2->set_geometry(P);

  // get the two poses out
  Pose3d pose1 = *P->get_pose(g1); 
  Pose3d pose2 = *P->get_pose(g2); 

  // relative poses will be identical
  assert((pose1.x - pose2.x).norm() < NEAR_ZERO &&
         Quatd::to_omega(pose1.q, pose2.q - pose1.q).norm() < NEAR_ZERO);

  // global pose will not necessarily be (depends on the global poses of g1 and g2)
  pose1.update_relative_pose(GLOBAL);
  pose2.update_relative_pose(GLOBAL);
  std::cout << "positional difference: " << (pose1.x - pose2.x) << std::endl;
  std::cout << "orientational difference (time derivative of quaternion): " << (pose1.q - pose2.q) << std::endl;
}