From 958c705a025a42fc11a23f5d7c9c078b472cf5c3 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Wed, 25 Aug 2010 01:28:46 +0000 Subject: [PATCH] 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 | 82 ++++++ .../ConvexHullDistanceDemo.cpp | 260 +++++++++++++++--- .../BroadphaseCollision/btDispatcher.h | 2 + .../btConvexConvexAlgorithm.cpp | 8 +- .../btParallelConstraintSolver.h | 4 +- 5 files changed, 322 insertions(+), 34 deletions(-) create mode 100644 Demos/ConvexHullDistance/CMakeLists.txt diff --git a/Demos/ConvexHullDistance/CMakeLists.txt b/Demos/ConvexHullDistance/CMakeLists.txt new file mode 100644 index 000000000..872063bda --- /dev/null +++ b/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) \ No newline at end of file diff --git a/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp b/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp index cc5ff807d..cadfde764 100644 --- a/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp +++ b/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 @@ -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)}; - btPoint3 points1[5]={btPoint3(1,0,0),btPoint3(0,1,0),btPoint3(0,0,1),btPoint3(0,0,-1),btPoint3(-1,-1,0)}; + btVector3 points0[3]={btVector3(1,0,0),btVector3(0,1,0),btVector3(0,0,1)}; + 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 hullB(points1,5); + btConvexHullShape hullA(&points0[0].getX(),3); + 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"); -} - -//to be implemented by the demo - -void clientMoveAndDisplay() -{ - - clientDisplay(); + return myglutmain(argc, argv,screenWidth,screenHeight,"Convex Hull Distance Demo"); } @@ -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;igetMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + 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; } diff --git a/src/BulletMultiThreaded/btParallelConstraintSolver.h b/src/BulletMultiThreaded/btParallelConstraintSolver.h index 0ff820a4c..10f35ab21 100644 --- a/src/BulletMultiThreaded/btParallelConstraintSolver.h +++ b/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;