collision_points_object_order.cpp

collision_points_object_order.cpp

/* Test for collision points locations generated by purterbation rotations in Bullet Physics */

#define BT_USE_DOUBLE_PRECISION

#include <BulletCollision/CollisionDispatch/btCollisionObject.h>
#include <BulletCollision/CollisionDispatch/btCollisionWorld.h>
#include <BulletCollision/CollisionDispatch/btCollisionDispatcher.h>
#include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
#include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>

#include <stdio.h>

/** This class allows to iterate over all the contact points in a
 *  btCollisionWorld, returning a tuple containing the two btCollisionObjects
 *  and the btManifoldPoint.  To be called after
 *  performDiscreteCollisionDetection().
 */
class IterateContactPoints
{
public:
  btCollisionWorld* world;

  IterateContactPoints(btCollisionWorld* _world)
    : world(_world) {}

  struct ContactPointTuple
  {
    const btCollisionObject* objectA;
    const btCollisionObject* objectB;
    btManifoldPoint* point;
    btPersistentManifold* manifold;
  };

  class iterator {
  protected:
    btCollisionWorld* world;
    ContactPointTuple data;
    unsigned int numManifolds;
    unsigned int numContacts;
    unsigned int manifold_index;
    unsigned int contact_index;
    iterator(btCollisionWorld* _world)
    {
      numManifolds = 0;
      manifold_index = -1;
      contact_index = -1;
      numContacts = 0;
      world = _world;
      numManifolds = world->getDispatcher()->getNumManifolds();
      ++(*this);
    }
  public:
    const ContactPointTuple& operator*() {
      return data;
    };
    const ContactPointTuple* operator->() {
      return &data;
    };

    iterator()
      { numManifolds = 0;
        manifold_index = -1;
        contact_index = -1;
        numContacts = 0; }

    iterator& operator++() {
      if (numManifolds == 0)
        return *this;
      contact_index ++;
      while (contact_index >= numContacts)
      {
        manifold_index ++;
        if (manifold_index < numManifolds)
        {
          data.manifold = world->getDispatcher()->
            getManifoldByIndexInternal(manifold_index);
          data.objectA = data.manifold->getBody0();
          data.objectB = data.manifold->getBody1();
          numContacts = data.manifold->getNumContacts();
          contact_index = 0;
        }
        else
        {
          numManifolds = 0;
          return *this;
        }
      }
      data.point = &(data.manifold->getContactPoint(contact_index));
      return *this;
    };

    bool operator!=(const iterator &it) {
      if (it.numManifolds==0) return numManifolds!=0;
      return data.objectA != it.data.objectA
        || data.objectB != it.data.objectB
        || data.point != it.data.point;
    };
    friend class IterateContactPoints;
  };

  iterator begin() {
    return iterator(world);
  };

  iterator end() {
    return iterator();
  };
};

void collision_points_test(bool boxfirst)
{
  // Setup Bullet
  btDbvtBroadphase broadphase;
  btDefaultCollisionConfiguration config;
  config.setConvexConvexMultipointIterations(2,2);
  config.setPlaneConvexMultipointIterations(2,2);
  btCollisionDispatcher dispatcher(&config);
  btCollisionWorld world(&dispatcher, &broadphase, &config);

  btGImpactCollisionAlgorithm::registerAlgorithm(&dispatcher);
  world.getDispatchInfo().m_useContinuous = false;

  // Setup shapes
  double y = 0.19;
  double fy = 0.0;
  double margin = 0.02;
  double edge = 0.1;
  btBoxShape box(btVector3(edge*10,edge*10,edge));
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge*10, -edge*10, -edge+fy, edge+fy);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", edge*10, edge*10, -edge+fy, edge+fy);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge*10, edge*10, edge+fy, edge+fy);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge*10, edge*10, -edge+fy, -edge+fy);
  btConvexHullShape ch;
  ch.addPoint(btVector3(-edge+margin, edge-margin, -edge+margin));
  ch.addPoint(btVector3(-edge+margin, -edge+margin, -edge+margin));
  ch.addPoint(btVector3(-edge+margin, -edge+margin, edge-margin));
  ch.addPoint(btVector3(-edge+margin, edge-margin, edge-margin));
  ch.addPoint(btVector3(edge-margin, edge-margin, edge-margin));
  ch.addPoint(btVector3(edge-margin, edge-margin, -edge+margin));
  ch.addPoint(btVector3(edge-margin, -edge+margin, -edge+margin));
  ch.addPoint(btVector3(edge-margin, -edge+margin, edge-margin), true);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge, -edge, -edge+y, edge+y);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", edge, edge, -edge+y, edge+y);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge, edge, edge+y, edge+y);
  printf("plt.plot([%f,%f],[%f,%f],'k')\n", -edge, edge, -edge+y, -edge+y);
  printf("plt.plot([%f,%f],[%f,%f],'--k')\n", -edge+margin, -edge+margin, -edge+margin+y, edge-margin+y);
  printf("plt.plot([%f,%f],[%f,%f],'--k')\n", edge-margin, edge-margin, -edge+margin+y, edge-margin+y);
  printf("plt.plot([%f,%f],[%f,%f],'--k')\n", -edge+margin, edge-margin, edge-margin+y, edge-margin+y);
  printf("plt.plot([%f,%f],[%f,%f],'--k')\n", -edge+margin, edge-margin, -edge+margin+y, -edge+margin+y);
  box.setMargin(margin);
  ch.setMargin(margin);
  btCollisionObject objbox, objch;
  objbox.setCollisionShape(&box);
  objch.setCollisionShape(&ch);
  if (boxfirst)
  {
    world.addCollisionObject(&objbox);
    world.addCollisionObject(&objch);
  }
  else
  {
    world.addCollisionObject(&objch);
    world.addCollisionObject(&objbox);
  }
  world.updateAabbs();

  gContactBreakingThreshold = 0.02;
  objbox.setContactProcessingThreshold(0.03);
  objch.setContactProcessingThreshold(0.03);
  objbox.setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

  // Set positions
  // Convex hull should be right on top of box
  objbox.getWorldTransform().setOrigin(btVector3(0,0,fy));
  objch.getWorldTransform().setOrigin(btVector3(0,0,y));

  // Do collision detection
  world.performDiscreteCollisionDetection();

  // Output points
  for (const auto it : IterateContactPoints(&world))
  {
    fprintf(stderr, "A is %s, ", it.manifold->getBody0()==&objbox ? "box" : "ch");
    fprintf(stderr, "B is %s\n", it.manifold->getBody1()==&objch ? "ch" : "box");
    btVector3 a(it.point->getPositionWorldOnA()),
      b(it.point->getPositionWorldOnB());
    printf("plt.plot([%f,%f], [%f,%f], '-or')\n", a.x(), b.x(), a.z(), b.z());
    printf("plt.plot([%f], [%f], 'ob')\n", a.x(), a.z());
    // printf("absolute: %f, %f, %f | %f, %f, %f\n",
    //        a.x(), a.y(), a.z(), b.x(), b.y(), b.z());
    a = it.point->m_localPointA;
    b = it.point->m_localPointB;
    // printf("relative: %f, %f, %f | %f, %f, %f\n",
    //        a.x(), a.y(), a.z(), b.x(), b.y(), b.z());
  }

  printf("plt.xlim(-0.15,0.15)\n");
  printf("plt.ylim(0.05,0.15)\n");
}

int main()
{
  printf("from matplotlib import pyplot as plt\n");
  printf("plt.figure(figsize=(12,3))\n");
  printf("plt.subplot(1,2,1,aspect=1)\n");
  printf("plt.title('box then convex hull')\n");
  collision_points_test(true);
  printf("plt.subplot(1,2,2,aspect=1)\n");
  printf("plt.title('convex hull then box')\n");
  collision_points_test(false);
  printf("plt.show()\n");
  return 0;
}

Run with make && ./collision_points_object_order | python -