double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
{
btVector3 vp(p1[0], p1[1], p1[2]);
btTriangleShape trishapeA(vp,
btVector3(p2[0], p2[1], p2[2]),
btVector3(p3[0], p3[1], p3[2]));
trishapeA.setMargin(0.000001f);
btVector3 vq(q1[0], q1[1], q1[2]);
btTriangleShape trishapeB(vq,
btVector3(q2[0], q2[1], q2[2]),
btVector3(q3[0], q3[1], q3[2]));
trishapeB.setMargin(0.000001f);
// btVoronoiSimplexSolver sGjkSimplexSolver;
// btGjkEpaPenetrationDepthSolver penSolverPtr;
static btSimplexSolverInterface sGjkSimplexSolver;
sGjkSimplexSolver.reset();
static btGjkEpaPenetrationDepthSolver Solver0;
static btMinkowskiPenetrationDepthSolver Solver1;
btConvexPenetrationDepthSolver* Solver = NULL;
Solver = &Solver1;
btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
convexConvex.m_catchDegeneracies = 1;
// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
btTransform tr;
tr.setIdentity();
input.m_transformA = tr;
input.m_transformB = tr;
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
normal[0] = gjkOutput.m_normalOnBInWorld[0];
normal[1] = gjkOutput.m_normalOnBInWorld[1];
normal[2] = gjkOutput.m_normalOnBInWorld[2];
return gjkOutput.m_distance;
}
return -1.0f;
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
ATTRIBUTE_ALIGNED16(btScalar) m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
gContactBreakingThreshold=1e10f;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
if (debugMode)
{
/// for polyhedral shapes
if (shapePtr[i]->isPolyhedral())
{
if (!shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = new btConvexHullComputer();
shapePtr[i]->setUserPointer(convexUtil);
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shapePtr[i];
btAlignedObjectArray<btVector3> vertices;
vertices.resize(polyshape->getNumVertices());
for (int i=0;i<polyshape->getNumVertices();i++)
{
polyshape->getVertex(i,vertices[i]);
}
bool useDoublePrecision = false;
convexUtil->compute(&vertices[0].getX(),sizeof(btVector3), polyshape->getNumVertices(),0,0);
}
if (shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = (btConvexHullComputer*)shapePtr[i]->getUserPointer();
//printf("num faces = %d\n",convexUtil->faces.size());
for (int j=0;j<convexUtil->faces.size();j++)
{
int face = convexUtil->faces[j];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
do
{
int src = edge->getSourceVertex();
int targ = edge->getTargetVertex();
//printf("src=%d target = %d\n", src,targ);
btVector3 wa = tr[i] * convexUtil->vertices[src];
btVector3 wb = tr[i] * convexUtil->vertices[targ];
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3f(wa.getX(),wa.getY(),wa.getZ());
glVertex3f(wb.getX(),wb.getY(),wb.getZ());
glEnd();
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
}
}
}
} else
{
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
bool BulletModel::findClosestPointsBtwElements(const ElementId idA,
const ElementId idB,
const bool use_margins,
std::unique_ptr<ResultCollector>& c)
{
btConvexShape* shapeA;
btConvexShape* shapeB;
btGjkPairDetector::ClosestPointInput input;
btPointCollector gjkOutput;
BulletCollisionWorldWrapper& bt_world = getBulletWorld(use_margins);
auto bt_objA_iter = bt_world.bt_collision_objects.find(idA);
if (bt_objA_iter == bt_world.bt_collision_objects.end())
return false;
auto bt_objB_iter = bt_world.bt_collision_objects.find(idB);
if (bt_objB_iter == bt_world.bt_collision_objects.end())
return false;
unique_ptr<btCollisionObject>& bt_objA = bt_objA_iter->second;
unique_ptr<btCollisionObject>& bt_objB = bt_objB_iter->second;
shapeA = (btConvexShape*) bt_objA->getCollisionShape();
shapeB = (btConvexShape*) bt_objB->getCollisionShape();
btGjkEpaPenetrationDepthSolver epa;
btVoronoiSimplexSolver sGjkSimplexSolver;
sGjkSimplexSolver.setEqualVertexThreshold(0.f);
btGjkPairDetector convexConvex(shapeA,shapeB,&sGjkSimplexSolver,&epa);
input.m_transformA = bt_objA->getWorldTransform();
input.m_transformB = bt_objB->getWorldTransform();
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 pointOnAinWorld;
btVector3 pointOnBinWorld;
if (elements[idA]->getShape() == DrakeShapes::MESH ||
elements[idA]->getShape() == DrakeShapes::MESH_POINTS ||
elements[idA]->getShape() == DrakeShapes::BOX) {
pointOnAinWorld = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*(gjkOutput.m_distance+shapeA->getMargin());
} else {
pointOnAinWorld = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
}
if (elements[idB]->getShape() == DrakeShapes::MESH ||
elements[idB]->getShape() == DrakeShapes::MESH_POINTS ||
elements[idB]->getShape() == DrakeShapes::BOX) {
pointOnBinWorld = gjkOutput.m_pointInWorld - gjkOutput.m_normalOnBInWorld*shapeB->getMargin();
} else {
pointOnBinWorld = gjkOutput.m_pointInWorld;
}
btVector3 pointOnElemA = input.m_transformA.invXform(pointOnAinWorld);
btVector3 pointOnElemB = input.m_transformB.invXform(pointOnBinWorld);
VectorXd pointOnA_1(4);
VectorXd pointOnB_1(4);
pointOnA_1 << toVector3d(pointOnElemA), 1;
pointOnB_1 << toVector3d(pointOnElemB), 1;
Vector3d pointOnA;
Vector3d pointOnB;
pointOnA << elements[idA]->getLocalTransform().topRows(3)*pointOnA_1;
pointOnB << elements[idB]->getLocalTransform().topRows(3)*pointOnB_1;
btScalar distance = gjkOutput.m_normalOnBInWorld.dot(pointOnAinWorld-pointOnBinWorld);
if (gjkOutput.m_hasResult) {
c->addSingleResult(idA,idB,pointOnA,pointOnB, toVector3d(gjkOutput.m_normalOnBInWorld),(double) distance);
} else {
c->addSingleResult(idA,
idB,
Vector3d::Zero(),
Vector3d::Zero(),
Vector3d::Zero(),1.0);
cerr << "In BulletModel::findClosestPointsBtwElements: No closest point found!" << endl;
}
return (c->pts.size() > 0);
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
float m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void clientDisplay(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.SetCachedSeperatingAxis(seperatingAxis);
PointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.GetClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
SimdVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
for (i=0; i<numObjects; i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
simplex.SetSimplexSolver(&sGjkSimplexSolver);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset();
for (i=0; i<numpoints; i++)
simplex.AddVertex(ybuf[i]);
SimdTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode());
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
// pitch += 0.005f;
// yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void CollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
btVoronoiSimplexSolver sGjkSimplexSolver;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
//VECCOPY(pa, gjkOutput.m_pointInWorld);
//VECCOPY(pb, gjkOutput.m_pointInWorld);
//VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance);
printf("bullet: %10.10f\n", gjkOutput.m_distance); // = 0.24 => that's absolutely wrong!
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
// btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
//let it rotate
//tr[0].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
