Added Genscher's triangle-triangle case (it was not a but)

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);
+//	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];
 
 	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<numObjects;i++)
 	{
@@ -180,7 +251,8 @@ void CollisionDemo::displayCallback(void) {
 
 	btQuaternion orn;
 	orn.setEuler(yaw,pitch,roll);
-	tr[0].setRotation(orn);
+	//let it rotate
+	//tr[0].setRotation(orn);
 
 	pitch += 0.005f;
 	yaw += 0.01f;