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