void SimplexDemo::initPhysics() { simplex.setSimplexSolver(&simplexSolver); simplex.addVertex(btVector3(-2,0,-2)); simplex.addVertex(btVector3(2,0,-2)); simplex.addVertex(btVector3(0,0,2)); simplex.addVertex(btVector3(0,2,0)); shapePtr[0] = &simplex; btTransform tr; tr.setIdentity(); }
int main(int argc,char** argv) { raytracePicture = new RenderTexture(screenWidth,screenHeight); myBox.SetMargin(0.02f); myCone.SetMargin(0.2f); simplex.SetSimplexSolver(&simplexSolver); simplex.AddVertex(SimdPoint3(-1,0,-1)); simplex.AddVertex(SimdPoint3(1,0,-1)); simplex.AddVertex(SimdPoint3(0,0,1)); simplex.AddVertex(SimdPoint3(0,1,0)); /// convex hull of 5 spheres #define NUM_SPHERES 5 SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); SimdVector3 positions[NUM_SPHERES] = { SimdVector3(-1.2f, -0.3f, 0.f), SimdVector3(0.8f, -0.3f, 0.f), SimdVector3(0.5f, 0.6f, 0.f), SimdVector3(-0.5f, 0.6f, 0.f), SimdVector3(0.f, 0.f, 0.f) }; SimdScalar radi[NUM_SPHERES] = { 0.35f,0.35f,0.45f,0.40f,0.40f }; MultiSphereShape multiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); ConvexHullShape convexHullShape(positions,3); //choose shape shapePtr[0] = &myCone; shapePtr[1] =&simplex; shapePtr[2] =&convexHullShape; shapePtr[3] =&myMink;//myBox; simplex.SetMargin(0.3f); setCameraDistance(6.f); return glutmain(argc, argv,screenWidth,screenHeight,"Minkowski-Sum Raytracer Demo"); }
void Raytracer::initPhysics() { raytracePicture = new RenderTexture(screenWidth,screenHeight); myBox.SetMargin(0.02f); myCone.SetMargin(0.2f); simplex.SetSimplexSolver(&simplexSolver); simplex.AddVertex(SimdPoint3(-1,0,-1)); simplex.AddVertex(SimdPoint3(1,0,-1)); simplex.AddVertex(SimdPoint3(0,0,1)); simplex.AddVertex(SimdPoint3(0,1,0)); /// convex hull of 5 spheres #define NUM_SPHERES 5 SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); SimdVector3 positions[NUM_SPHERES] = { SimdVector3(-1.2f, -0.3f, 0.f), SimdVector3(0.8f, -0.3f, 0.f), SimdVector3(0.5f, 0.6f, 0.f), SimdVector3(-0.5f, 0.6f, 0.f), SimdVector3(0.f, 0.f, 0.f) }; // MultiSphereShape* multiSphereShape = new MultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); ConvexHullShape* convexHullShape = new ConvexHullShape(positions,3); //choose shape shapePtr[0] = &myCone; shapePtr[1] =&simplex; shapePtr[2] =convexHullShape; shapePtr[3] =&myMink;//myBox;//multiSphereShape simplex.SetMargin(0.3f); }
void SimplexDemo::displayCallback() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); GL_ShapeDrawer::drawCoordSystem(); btScalar m[16]; int i; btVector3 worldBoundsMin(-1000,-1000,-1000); btVector3 worldBoundsMax(1000,1000,1000); for (i=0; i<numObjects; i++) { btTransform transA; transA.setIdentity(); btVector3 dpos(0.f,5.f,0.f); transA.setOrigin( dpos ); btQuaternion orn; orn.setEuler(yaw,pitch,roll); transA.setRotation(orn); transA.getOpenGLMatrix( m ); /// draw the simplex m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax); /// calculate closest point from simplex to the origin, and draw this vector simplex.calcClosest(m); } 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 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(); }