Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp to compile again

Allow to use convexProcessingThreshold as maximum distance for convex-convex computation (useful for closest distance/point computation)

Demos/ConvexHullDistance/CMakeLists.txt

@@ -0,0 +1,82 @@
+# This is basically the overall name of the project in Visual Studio this is the name of the Solution File
+
+
+# For every executable you have with a main method you should have an add_executable line below.
+# For every add executable line you should list every .cpp and .h file you have associated with that executable.  
+
+
+# This is the variable for Windows.  I use this to define the root of my directory structure.
+SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut)
+
+# You shouldn't have to modify anything below this line 
+########################################################
+
+INCLUDE_DIRECTORIES(
+${BULLET_PHYSICS_SOURCE_DIR}/src ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL 
+)
+
+
+
+IF (USE_GLUT)
+	LINK_LIBRARIES(
+	OpenGLSupport  BulletDynamics  BulletCollision LinearMath  ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+	)
+
+IF (WIN32)
+ADD_EXECUTABLE(AppConvexHullDistanceDemo
+
+		ConvexHullDistanceDemo.cpp
+		${BULLET_PHYSICS_SOURCE_DIR}/msvc/bullet.rc
+	)
+ELSE()
+	ADD_EXECUTABLE(AppConvexHullDistanceDemo
+		ConvexHullDistanceDemo.cpp
+	)
+ENDIF()
+
+
+
+
+	IF (WIN32)
+		IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+			IF (CMAKE_CL_64)
+				ADD_CUSTOM_COMMAND(
+					TARGET AppConvexHullDistanceDemo
+					POST_BUILD
+					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR}
+				)
+			ELSE(CMAKE_CL_64)
+				ADD_CUSTOM_COMMAND(
+					TARGET AppConvexHullDistanceDemo
+					POST_BUILD
+#		  		COMMAND copy /Y ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
+					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
+				)
+			ENDIF(CMAKE_CL_64)
+		ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+
+	ENDIF(WIN32)
+ELSE (USE_GLUT)
+
+ 
+
+	LINK_LIBRARIES(
+	OpenGLSupport  BulletDynamics  BulletCollision LinearMath  ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+	)
+
+
+	ADD_EXECUTABLE(AppConvexHullDistanceDemo
+		WIN32
+		../OpenGL/Win32AppMain.cpp
+		ConvexHullDistanceDemo.cpp
+		${BULLET_PHYSICS_SOURCE_DIR}/msvc/bullet.rc
+	)
+	
+	
+ENDIF (USE_GLUT)
+
+IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
+			SET_TARGET_PROPERTIES(AppConvexHullDistanceDemo PROPERTIES  DEBUG_POSTFIX "_Debug")
+			SET_TARGET_PROPERTIES(AppConvexHullDistanceDemo PROPERTIES  MINSIZEREL_POSTFIX "_MinsizeRel")
+			SET_TARGET_PROPERTIES(AppConvexHullDistanceDemo PROPERTIES  RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo")
+ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)

Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp

@@ -24,12 +24,20 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
+#include "BulletCollision/CollisionShapes/btCylinderShape.h"
 
 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletDynamics/NarrowPhaseCollision/btPointCollector.h"
+#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
+#include "LinearMath/btIDebugDraw.h"
 
+//#define USE_GJK
+
+#ifndef USE_GJK
+#include "btBulletCollisionCommon.h"
+
+#endif //USE_GJK
 #include "GL_ShapeDrawer.h"
 #ifdef WIN32 //needed for glut.h
 #include <windows.h>
@@ -51,12 +59,119 @@ const int numObjects = 2;
 
 GL_Simplex1to4 simplex;
 
-btPolyhedralConvexShape*	shapePtr[maxNumObjects];
+btConvexShape*	shapePtr[maxNumObjects];
 
 btTransform tr[numObjects];
 int screenWidth = 640.f;
 int screenHeight = 480.f;
 
+void clientResetScene()
+{
+	tr[0].setOrigin(btVector3(0.0f,3.f,7.f));
+	tr[1].setOrigin(btVector3(0.0f,9.f,2.f));
+}
+
+int debugMode = 0;//btIDebugDraw::DBG_DrawWireframe;
+GL_ShapeDrawer shapeDrawer;
+int m_glutScreenWidth=0;
+int m_glutScreenHeight=0;
+float m_frustumZNear = 1.f;
+float m_frustumZFar = 10000.f;
+bool m_ortho = false;
+
+int myglutmain(int argc, char **argv,int width,int height,const char* title);
+
+
+void updateCamera() {
+
+
+	glMatrixMode(GL_PROJECTION);
+	glLoadIdentity();
+	btScalar rele = 0;
+	btScalar razi = 0;
+	btVector3 m_cameraUp(0,1,0);
+	btScalar m_cameraDistance = 10.f;
+	btVector3 m_cameraPosition;
+	
+
+	btVector3 m_cameraTargetPosition = (tr[0].getOrigin()+tr[1].getOrigin())*0.5;
+
+
+	btQuaternion rot(m_cameraUp,razi);
+
+
+	int m_forwardAxis = 2;
+
+	btVector3 eyePos(0,0,0);
+	eyePos[m_forwardAxis] = -m_cameraDistance;
+
+	btVector3 forward(eyePos[0],eyePos[1],eyePos[2]);
+	if (forward.length2() < SIMD_EPSILON)
+	{
+		forward.setValue(1.f,0.f,0.f);
+	}
+	btVector3 right = m_cameraUp.cross(forward);
+	btQuaternion roll(right,-rele);
+
+	eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;
+
+	m_cameraPosition[0] = eyePos.getX();
+	m_cameraPosition[1] = eyePos.getY();
+	m_cameraPosition[2] = eyePos.getZ();
+	m_cameraPosition += m_cameraTargetPosition;
+
+	if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0)
+		return;
+
+	btScalar aspect;
+	btVector3 extents;
+
+	if (m_glutScreenWidth > m_glutScreenHeight) 
+	{
+		aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
+		extents.setValue(aspect * 1.0f, 1.0f,0);
+	} else 
+	{
+		aspect = m_glutScreenHeight / (btScalar)m_glutScreenWidth;
+		extents.setValue(1.0f, aspect*1.f,0);
+	}
+
+	
+	if (m_ortho)
+	{
+		// reset matrix
+		glLoadIdentity();
+		
+		
+		extents *= m_cameraDistance;
+		btVector3 lower = m_cameraTargetPosition - extents;
+		btVector3 upper = m_cameraTargetPosition + extents;
+		//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
+		glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000);
+		
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		//glTranslatef(100,210,0);
+	} else
+	{
+		if (m_glutScreenWidth > m_glutScreenHeight) 
+		{
+//			glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0);
+			glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
+		} else 
+		{
+//			glFrustum (-1.0, 1.0, -aspect, aspect, 1.0, 10000.0);
+			glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar);
+		}
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], 
+			m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], 
+			m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ());
+	}
+
+}
+
 
 int main(int argc,char** argv)
 {
@@ -71,13 +186,14 @@ int main(int argc,char** argv)
 	tr[0].setBasis(basisA);
 	tr[1].setBasis(basisB);
 
-	btPoint3	points0[3]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1)};
+	btVector3	points0[3]={btVector3(1,0,0),btVector3(0,1,0),btVector3(0,0,1)};
-	btPoint3	points1[5]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1),btPoint3(0,0,-1),btPoint3(-1,-1,0)};
+	btVector3	points1[5]={btVector3(1,0,0),btVector3(0,1,0),btVector3(0,0,1),btVector3(0,0,-1),btVector3(-1,-1,0)};
 	
-	btConvexHullShape	hullA(points0,3);
+	btConvexHullShape	hullA(&points0[0].getX(),3);
-	btConvexHullShape	hullB(points1,5);
+	btConvexHullShape	hullB(&points1[0].getX(),5);
+	btCylinderShape cylinder(btVector3(0.3,1,1));
 
-	shapePtr[0] = &hullA;
+	shapePtr[0] = &cylinder;//hullA;
 	shapePtr[1] = &hullB;
 	
 
@@ -85,15 +201,7 @@ int main(int argc,char** argv)
 	tr.setIdentity();
 
 
-	return glutmain(argc, argv,screenWidth,screenHeight,"Convex Hull Distance Demo");
+	return myglutmain(argc, argv,screenWidth,screenHeight,"Convex Hull Distance Demo");
-}
-
-//to be implemented by the demo
-
-void clientMoveAndDisplay()
-{
-	
-	clientDisplay();
 }
 
 
@@ -104,6 +212,9 @@ btSimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver;
 
 void clientDisplay(void) {
 
+
+	updateCamera();
+
     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
 	glDisable(GL_LIGHTING);
 
@@ -111,7 +222,7 @@ void clientDisplay(void) {
 
 	float m[16];
 	int i;
-
+#ifdef USE_GJK
 	btGjkPairDetector	convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
 
 	btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
@@ -136,19 +247,55 @@ void clientDisplay(void) {
 		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 );
 
-		GL_ShapeDrawer::drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode());
+		shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
 
 
 	}
 
 	simplex.setSimplexSolver(&sGjkSimplexSolver);
-	btPoint3 ybuf[4],pbuf[4],qbuf[4];
+	btVector3 ybuf[4],pbuf[4],qbuf[4];
 	int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
 	simplex.reset();
 	
@@ -158,7 +305,7 @@ void clientDisplay(void) {
 	btTransform ident;
 	ident.setIdentity();
 	ident.getOpenGLMatrix(m);
-	GL_ShapeDrawer::drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode());
+	shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
 
 
 	btQuaternion orn;
@@ -173,23 +320,74 @@ void clientDisplay(void) {
     glutSwapBuffers();
 }
 
-void clientResetScene()
+void clientMoveAndDisplay()
 {
-	tr[0].setOrigin(btVector3(0.0f,3.f,7.f));
+	clientDisplay();
-	tr[1].setOrigin(btVector3(0.0f,9.f,2.f));
 }
 
-void clientKeyboard(unsigned char key, int x, int y)
+static void glutReshapeCallback(int w, int h)
 {
-	defaultKeyboard(key, x, y);
+	m_glutScreenWidth=w;
+	m_glutScreenHeight=h;
 }
 
 
-void clientMouseFunc(int button, int state, int x, int y)
+int myglutmain(int argc, char **argv,int width,int height,const char* title) {
-{
     
-}
 
-void	clientMotionFunc(int x,int y)
+	glutInit(&argc, argv);
-{
+    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_STENCIL);
+    glutInitWindowPosition(0, 0);
+    glutInitWindowSize(width, height);
+	
+    glutCreateWindow(title);
+#ifdef BT_USE_FREEGLUT
+	glutSetOption (GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_GLUTMAINLOOP_RETURNS);
+#endif
+
+
+	glutIdleFunc( clientDisplay );
+	glutDisplayFunc( clientDisplay );
+	glutReshapeFunc(glutReshapeCallback);
+
+//enable vsync to avoid tearing on Apple (todo: for Windows)
+
+#if defined(__APPLE__) && !defined (VMDMESA)
+int swap_interval = 1;
+CGLContextObj cgl_context = CGLGetCurrentContext();
+CGLSetParameter(cgl_context, kCGLCPSwapInterval, &swap_interval);
+#endif
+
+	GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) };
+	GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) };
+	GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )};
+	/*	light_position is NOT default value	*/
+	GLfloat light_position0[] = { btScalar(1.0), btScalar(10.0), btScalar(1.0), btScalar(0.0 )};
+	GLfloat light_position1[] = { btScalar(-1.0), btScalar(-10.0), btScalar(-1.0), btScalar(0.0) };
+
+	glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
+	glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
+	glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
+	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
+
+	glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
+	glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
+	glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
+	glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
+
+	glEnable(GL_LIGHTING);
+	glEnable(GL_LIGHT0);
+	glEnable(GL_LIGHT1);
+
+
+	glShadeModel(GL_SMOOTH);
+	glEnable(GL_DEPTH_TEST);
+	glDepthFunc(GL_LESS);
+
+	glClearColor(btScalar(0.7),btScalar(0.7),btScalar(0.7),btScalar(0));
+
+
+	
+    glutMainLoop();
+    return 0;
 }

src/BulletCollision/BroadphaseCollision/btDispatcher.h

@@ -48,6 +48,7 @@ struct btDispatcherInfo
 		m_allowedCcdPenetration(btScalar(0.04)),
 		m_useConvexConservativeDistanceUtil(false),
 		m_convexConservativeDistanceThreshold(0.0f),
+		m_convexMaxDistanceUseCPT(false),
 		m_stackAllocator(0)
 	{
 
@@ -64,6 +65,7 @@ struct btDispatcherInfo
 	btScalar	m_allowedCcdPenetration;
 	bool		m_useConvexConservativeDistanceUtil;
 	btScalar	m_convexConservativeDistanceThreshold;
+	bool		m_convexMaxDistanceUseCPT;
 	btStackAlloc*	m_stackAllocator;
 };
 

src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

@@ -356,8 +356,14 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
 		input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
 	} else
 #endif //USE_SEPDISTANCE_UTIL2
+	{
+		if (dispatchInfo.m_convexMaxDistanceUseCPT)
+		{
+			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
+		} else
 		{
 			input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
+		}
 		input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 	}
 

src/BulletMultiThreaded/btParallelConstraintSolver.h

@@ -135,7 +135,7 @@ ATTRIBUTE_ALIGNED16(struct) PfxSolverBody {
 
 static SIMD_FORCE_INLINE vmVector3 btReadVector3(const double* p)
 {
-	float tmp[3] = {p[0],p[1],p[2]};
+	float tmp[3] = {float(p[0]),float(p[1]),float(p[2])};
 	vmVector3 v;
 	loadXYZ(v, tmp);
 	return v;
@@ -143,7 +143,7 @@ static SIMD_FORCE_INLINE vmVector3 btReadVector3(const double* p)
 
 static SIMD_FORCE_INLINE vmQuat btReadQuat(const double* p)
 {
-	float tmp[4] = {p[0],p[1],p[2],p[4]};
+	float tmp[4] = {float(p[0]),float(p[1]),float(p[2]),float(p[4])};
 	vmQuat vq;
 	loadXYZW(vq, tmp);
 	return vq;