diff --git a/Demos/CollisionDemo/CollisionDemo.cpp b/Demos/CollisionDemo/CollisionDemo.cpp index 51473a81c..0b7fb238b 100644 --- a/Demos/CollisionDemo/CollisionDemo.cpp +++ b/Demos/CollisionDemo/CollisionDemo.cpp @@ -20,6 +20,9 @@ subject to the following restrictions: /// +#define CHECK_GENSHER_TRIANGLE_CASE 1 + + ///This low-level internal demo does intentionally NOT use the btBulletCollisionCommon.h header ///It needs internal access #include "GL_Simplex1to4.h" @@ -31,6 +34,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btPointCollector.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "CollisionDemo.h" #include "GL_ShapeDrawer.h" @@ -59,8 +63,12 @@ int main(int argc,char** argv) { CollisionDemo* colDemo = new CollisionDemo(); +#ifdef CHECK_GENSHER_TRIANGLE_CASE + colDemo->setCameraDistance(8.f); +#else colDemo->setCameraDistance(18.f); - + +#endif // colDemo->initPhysics(); @@ -71,7 +79,11 @@ int main(int argc,char** argv) void CollisionDemo::initPhysics() { m_debugMode |= btIDebugDraw::DBG_DrawWireframe; +#ifdef CHECK_GENSHER_TRIANGLE_CASE + m_azi = 140.f; +#else m_azi = 250.f; +#endif m_ele = 25.f; tr[0].setOrigin(btVector3(0.0013328250f,8.1363249f,7.0390840f)); @@ -93,15 +105,62 @@ void CollisionDemo::initPhysics() tr[0].setBasis(basisA); tr[1].setBasis(basisB); - +#ifdef CHECK_GENSHER_TRIANGLE_CASE + tr[0].setIdentity(); + tr[1].setIdentity(); +#endif //CHECK_GENSHER_TRIANGLE_CASE btVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f); btVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f); btBoxShape* boxA = new btBoxShape(boxHalfExtentsA); btBoxShape* boxB = new btBoxShape(boxHalfExtentsB); + + float p1[3], p2[3], p3[3], q1[3],q2[3], q3[3]; + + p1[0] = -3.9242966175; + p1[1] = -0.5582823175; + p1[2] = 2.0101921558; + + p2[0] = -3.6731941700; + p2[1] = -0.5604774356; + p2[2] = 2.00301921558; + + p3[0] = -3.6698703766; + p3[1] = -0.3097069263; + p3[2] = 2.0073683262; + + + + + q1[0] = -2.6317186356; + q1[1] = -1.0000005960; + q1[2] = 1.9999998808; + + q2[0] = -2.6317174435; + q2[1] = 0.9999994636; + q2[2] = 1.9999998808; + + q3[0] = -4.6317176819; + q3[1] = 1.0f; + q3[2] = 1.9999998808; + + + + btTriangleShape* trishapeA = new btTriangleShape(btVector3(p1[0], p1[1], p1[2]), btVector3(p2[0], p2[1], p2[2]), btVector3(p3[0], p3[1], p3[2])); + trishapeA->setMargin(0.001f); + + btTriangleShape* trishapeB = new btTriangleShape(btVector3(q1[0], q1[1], q1[2]), btVector3(q2[0], q2[1], q2[2]), btVector3(q3[0], q3[1], q3[2])); + trishapeB->setMargin(0.001f); + + +#ifdef CHECK_GENSHER_TRIANGLE_CASE + shapePtr[0] = trishapeA;//boxA; + shapePtr[1] = trishapeB;//boxB; +#else shapePtr[0] = boxA; shapePtr[1] = boxB; +#endif } @@ -122,37 +181,49 @@ void CollisionDemo::displayCallback(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); - //GL_ShapeDrawer::drawCoordSystem(); + + + + 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(); float m[16]; int i; - btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); - - 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]; +// btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); 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()); - //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