Created
February 16, 2018 14:03
-
-
Save radarsat1/2eb69623b46ee10dfdb52ac617bcc658 to your computer and use it in GitHub Desktop.
collision_points_object_order.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* 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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Run with
make && ./collision_points_object_order | python -