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(); }