From 8acadeb71187437019d0c00d8d2c05f79ec1bc81 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Wed, 18 Feb 2009 22:52:03 +0000 Subject: [PATCH] minor tweaks to demos: enable constraint debug drawing in AllBulletDemos, default constraint debugging size set to 0.3, set svn:eol-style native for folder files http://code.google.com/p/bullet/issues/detail?id=191 --- CMakeLists.txt | 142 +- Demos/AllBulletDemos/DemoEntries.cpp | 4 +- Demos/AllBulletDemos/Main.cpp | 33 + Demos/ColladaDemo/ColladaDemo.cpp | 2 +- Demos/RagdollDemo/RagdollDemo.cpp | 22 + .../BroadphaseCollision/btDbvt.cpp | 2590 ++++---- .../BroadphaseCollision/btDbvt.h | 2510 ++++---- .../BroadphaseCollision/btDbvtBroadphase.cpp | 1454 ++--- .../BroadphaseCollision/btDbvtBroadphase.h | 252 +- .../btMultiSapBroadphase.cpp | 978 +-- .../btMultiSapBroadphase.h | 302 +- .../btOverlappingPairCache.cpp | 1266 ++-- .../btOverlappingPairCache.h | 936 +-- .../BroadphaseCollision/btQuantizedBvh.cpp | 2296 +++---- .../BroadphaseCollision/btQuantizedBvh.h | 946 +-- .../SphereTriangleDetector.cpp | 418 +- .../btActivatingCollisionAlgorithm.cpp | 94 +- .../btActivatingCollisionAlgorithm.h | 72 +- .../btBoxBoxCollisionAlgorithm.cpp | 170 +- .../btBoxBoxCollisionAlgorithm.h | 132 +- .../CollisionDispatch/btBoxBoxDetector.cpp | 1378 ++--- .../CollisionDispatch/btBoxBoxDetector.h | 88 +- .../btConvexPlaneCollisionAlgorithm.cpp | 310 +- .../btConvexPlaneCollisionAlgorithm.h | 168 +- .../CollisionDispatch/btGhostObject.cpp | 340 +- .../CollisionDispatch/btGhostObject.h | 346 +- .../btSimulationIslandManager.cpp | 780 +-- .../btSphereBoxCollisionAlgorithm.cpp | 520 +- .../btSphereBoxCollisionAlgorithm.h | 150 +- .../btSphereSphereCollisionAlgorithm.cpp | 210 +- .../btSphereSphereCollisionAlgorithm.h | 132 +- .../btSphereTriangleCollisionAlgorithm.cpp | 168 +- .../btSphereTriangleCollisionAlgorithm.h | 138 +- .../CollisionShapes/btCapsuleShape.cpp | 342 +- .../CollisionShapes/btCapsuleShape.h | 236 +- .../btConvexPointCloudShape.cpp | 312 +- .../CollisionShapes/btConvexPointCloudShape.h | 192 +- .../btConvexTriangleMeshShape.cpp | 628 +- .../btHeightfieldTerrainShape.cpp | 822 +-- .../btHeightfieldTerrainShape.h | 322 +- .../CollisionShapes/btMaterial.h | 66 +- .../btMultimaterialTriangleMeshShape.cpp | 90 +- .../btMultimaterialTriangleMeshShape.h | 246 +- .../btScaledBvhTriangleMeshShape.cpp | 240 +- .../btScaledBvhTriangleMeshShape.h | 124 +- .../CollisionShapes/btShapeHull.cpp | 328 +- .../CollisionShapes/btShapeHull.h | 112 +- .../CollisionShapes/btTriangleBuffer.cpp | 70 +- .../CollisionShapes/btTriangleBuffer.h | 138 +- .../btTriangleIndexVertexArray.cpp | 1 + .../btTriangleIndexVertexMaterialArray.cpp | 173 +- .../btTriangleIndexVertexMaterialArray.h | 168 +- .../NarrowPhaseCollision/btGjkEpa2.cpp | 1886 +++--- .../NarrowPhaseCollision/btGjkEpa2.h | 142 +- .../btGjkEpaPenetrationDepthSolver.cpp | 108 +- .../btCharacterControllerInterface.h | 86 +- .../btKinematicCharacterController.cpp | 942 +-- .../btKinematicCharacterController.h | 232 +- .../ConstraintSolver/btSliderConstraint.cpp | 1634 ++--- .../ConstraintSolver/btSliderConstraint.h | 458 +- .../ConstraintSolver/btSolverBody.h | 358 +- .../ConstraintSolver/btTypedConstraint.cpp | 2 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 2856 ++++----- .../Dynamics/btDiscreteDynamicsWorld.h | 352 +- src/BulletDynamics/Dynamics/btDynamicsWorld.h | 272 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 486 +- .../Vehicle/btRaycastVehicle.cpp | 1458 ++--- src/BulletDynamics/Vehicle/btWheelInfo.cpp | 112 +- .../SequentialThreadSupport.cpp | 186 +- .../SequentialThreadSupport.h | 174 +- .../SpuContactManifoldCollisionAlgorithm.cpp | 138 +- .../SpuContactManifoldCollisionAlgorithm.h | 240 +- src/BulletMultiThreaded/SpuFakeDma.cpp | 418 +- .../SpuGatheringCollisionDispatcher.h | 138 +- .../SpuIntegrationTask/readme.txt | 2 +- .../SpuNarrowPhaseCollisionTask/Box.h | 328 +- .../SpuCollisionShapes.cpp | 1084 ++-- .../SpuCollisionShapes.h | 250 +- .../SpuContactResult.cpp | 462 +- .../SpuGatheringCollisionTask.cpp | 2320 ++++---- .../SpuGjkEpa2.cpp | 1702 +++--- .../boxBoxDistance.cpp | 2310 +++---- .../boxBoxDistance.h | 132 +- .../SpuNarrowPhaseCollisionTask/readme.txt | 2 +- .../SpuRaycastTask/SpuRaycastTask.cpp | 1572 ++--- .../SpuRaycastTask/SpuRaycastTask.h | 100 +- .../SpuSubSimplexConvexCast.cpp | 304 +- .../SpuRaycastTask/SpuSubSimplexConvexCast.h | 120 +- .../SpuRaycastTaskProcess.cpp | 378 +- .../SpuSampleTask/SpuSampleTask.cpp | 428 +- .../SpuSampleTask/SpuSampleTask.h | 108 +- .../SpuSampleTask/readme.txt | 2 +- .../Win32ThreadSupport.cpp | 518 +- src/BulletMultiThreaded/Win32ThreadSupport.h | 250 +- src/BulletMultiThreaded/vectormath2bullet.h | 146 +- src/BulletSoftBody/btSoftBody.cpp | 5298 ++++++++--------- src/BulletSoftBody/btSoftBody.h | 1682 +++--- .../btSoftBodyConcaveCollisionAlgorithm.cpp | 738 +-- .../btSoftBodyConcaveCollisionAlgorithm.h | 306 +- src/BulletSoftBody/btSoftBodyHelpers.cpp | 1718 +++--- src/BulletSoftBody/btSoftBodyHelpers.h | 238 +- src/BulletSoftBody/btSoftBodyInternals.h | 1794 +++--- ...oftBodyRigidBodyCollisionConfiguration.cpp | 268 +- ...tSoftBodyRigidBodyCollisionConfiguration.h | 96 +- .../btSoftRigidCollisionAlgorithm.cpp | 158 +- .../btSoftRigidCollisionAlgorithm.h | 150 +- .../btSoftRigidDynamicsWorld.cpp | 270 +- src/BulletSoftBody/btSoftRigidDynamicsWorld.h | 164 +- .../btSoftSoftCollisionAlgorithm.cpp | 92 +- .../btSoftSoftCollisionAlgorithm.h | 138 +- src/BulletSoftBody/btSparseSDF.h | 612 +- src/LinearMath/CMakeLists.txt | 102 +- src/LinearMath/btAlignedAllocator.cpp | 410 +- src/LinearMath/btAlignedAllocator.h | 214 +- src/LinearMath/btAlignedObjectArray.h | 884 +-- src/LinearMath/btConvexHull.cpp | 2348 ++++---- src/LinearMath/btConvexHull.h | 482 +- src/LinearMath/btDefaultMotionState.h | 76 +- src/LinearMath/btGeometryUtil.cpp | 370 +- src/LinearMath/btGeometryUtil.h | 84 +- src/LinearMath/btHashMap.h | 606 +- src/LinearMath/btMotionState.h | 80 +- src/LinearMath/btPoolAllocator.h | 204 +- src/LinearMath/btStackAlloc.h | 232 +- src/LinearMath/ibmsdk/Makefile | 78 +- src/btBulletCollisionCommon.h | 132 +- 126 files changed, 34617 insertions(+), 34560 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c968a713..0afbcda1c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,71 +1,71 @@ -cmake_minimum_required(VERSION 2.4) - -PROJECT(BULLET_PHYSICS) -SET(BULLET_VERSION 2.74) - -IF (NOT CMAKE_BUILD_TYPE) -# SET(CMAKE_BUILD_TYPE "Debug") - SET(CMAKE_BUILD_TYPE "Release") -ENDIF (NOT CMAKE_BUILD_TYPE) - - - -IF(COMMAND cmake_policy) - cmake_policy(SET CMP0003 NEW) -ENDIF(COMMAND cmake_policy) - - -# This is the shortcut to finding GLU, GLUT and OpenGL if they are properly installed on your system -# This should be the case. - -FIND_PACKAGE(OpenGL) -IF (OPENGL_FOUND) -MESSAGE("OPENGL FOUND") -MESSAGE(${OPENGL_LIBRARIES}) -ELSE (OPENGL_FOUND) -MESSAGE("OPENGL NOT FOUND") -SET(OPENGL_gl_LIBRARY opengl32) -SET(OPENGL_glu_LIBRARY glu32) -ENDIF (OPENGL_FOUND) - -# ADD_DEFINITIONS(-DBT_USE_FREEGLUT) - -FIND_PACKAGE(GLU) - -FIND_PACKAGE(GLUT) -IF (GLUT_FOUND) -MESSAGE("GLUT FOUND") -MESSAGE(${GLUT_glut_LIBRARY}) -ELSE (GLUT_FOUND) - -IF (MINGW) -MESSAGE ("GLUT NOT FOUND not found, trying to use MINGW glut32") -SET(GLUT_glut_LIBRARY glut32) -ENDIF (MINGW) - -IF (MSVC) -MESSAGE ("GLUT NOT FOUND, trying to use Bullet/Glut/glut32.lib for MSVC") -SET(GLUT_glut_LIBRARY ${BULLET_PHYSICS_SOURCE_DIR}/Glut/glut32.lib) -ENDIF (MSVC) -ENDIF (GLUT_FOUND) - - -IF (WIN32) - INCLUDE_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/Glut) -ELSE (WIN32) - # This is the lines for linux. This should always work if everything is installed and working fine. - INCLUDE_DIRECTORIES(/usr/include /usr/local/include ${GLUT_INCLUDE_DIR}) -ENDIF (WIN32) - - -OPTION(BUILD_DEMOS "Set when you want to build the demos" ON) -IF(BUILD_DEMOS) - SUBDIRS(Demos) -ENDIF(BUILD_DEMOS) - -OPTION(BUILD_EXTRAS "Set when you want to build the extras" ON) -IF(BUILD_EXTRAS) - SUBDIRS(Extras) -ENDIF(BUILD_EXTRAS) - -SUBDIRS(src) +cmake_minimum_required(VERSION 2.4) + +PROJECT(BULLET_PHYSICS) +SET(BULLET_VERSION 2.74) + +IF (NOT CMAKE_BUILD_TYPE) +# SET(CMAKE_BUILD_TYPE "Debug") + SET(CMAKE_BUILD_TYPE "Release") +ENDIF (NOT CMAKE_BUILD_TYPE) + + + +IF(COMMAND cmake_policy) + cmake_policy(SET CMP0003 NEW) +ENDIF(COMMAND cmake_policy) + + +# This is the shortcut to finding GLU, GLUT and OpenGL if they are properly installed on your system +# This should be the case. + +FIND_PACKAGE(OpenGL) +IF (OPENGL_FOUND) +MESSAGE("OPENGL FOUND") +MESSAGE(${OPENGL_LIBRARIES}) +ELSE (OPENGL_FOUND) +MESSAGE("OPENGL NOT FOUND") +SET(OPENGL_gl_LIBRARY opengl32) +SET(OPENGL_glu_LIBRARY glu32) +ENDIF (OPENGL_FOUND) + +# ADD_DEFINITIONS(-DBT_USE_FREEGLUT) + +FIND_PACKAGE(GLU) + +FIND_PACKAGE(GLUT) +IF (GLUT_FOUND) +MESSAGE("GLUT FOUND") +MESSAGE(${GLUT_glut_LIBRARY}) +ELSE (GLUT_FOUND) + +IF (MINGW) +MESSAGE ("GLUT NOT FOUND not found, trying to use MINGW glut32") +SET(GLUT_glut_LIBRARY glut32) +ENDIF (MINGW) + +IF (MSVC) +MESSAGE ("GLUT NOT FOUND, trying to use Bullet/Glut/glut32.lib for MSVC") +SET(GLUT_glut_LIBRARY ${BULLET_PHYSICS_SOURCE_DIR}/Glut/glut32.lib) +ENDIF (MSVC) +ENDIF (GLUT_FOUND) + + +IF (WIN32) + INCLUDE_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/Glut) +ELSE (WIN32) + # This is the lines for linux. This should always work if everything is installed and working fine. + INCLUDE_DIRECTORIES(/usr/include /usr/local/include ${GLUT_INCLUDE_DIR}) +ENDIF (WIN32) + + +OPTION(BUILD_DEMOS "Set when you want to build the demos" ON) +IF(BUILD_DEMOS) + SUBDIRS(Demos) +ENDIF(BUILD_DEMOS) + +OPTION(BUILD_EXTRAS "Set when you want to build the extras" ON) +IF(BUILD_EXTRAS) + SUBDIRS(Extras) +ENDIF(BUILD_EXTRAS) + +SUBDIRS(src) diff --git a/Demos/AllBulletDemos/DemoEntries.cpp b/Demos/AllBulletDemos/DemoEntries.cpp index 7152e3e15..ed82e3c93 100644 --- a/Demos/AllBulletDemos/DemoEntries.cpp +++ b/Demos/AllBulletDemos/DemoEntries.cpp @@ -103,13 +103,13 @@ public: btDemoEntry g_demoEntries[] = { - + {"ConstraintDemo",ConstraintDemo::Create}, {"ForkLift Demo",ForkLiftDemo::Create}, {"Ragdoll Demo",RagdollDemo::Create}, {"Basic Demo", BasicDemo::Create}, {"Convex Decomposition",ConvexDecompositionDemo::Create}, {"Concave Moving", GimpactConcaveDemo::Create}, - {"Dynamic Control Demo",MotorDemo::Create}, +// {"Dynamic Control Demo",MotorDemo::Create}, //{"ConcaveDemo",ConcaveDemo::Create}, {"Concave Convexcast Demo",ConcaveConvexcastDemo::Create}, // {"SoftBody Cloth",SoftDemo0::Create}, diff --git a/Demos/AllBulletDemos/Main.cpp b/Demos/AllBulletDemos/Main.cpp index c77a240ad..8d97f5a47 100644 --- a/Demos/AllBulletDemos/Main.cpp +++ b/Demos/AllBulletDemos/Main.cpp @@ -52,6 +52,7 @@ namespace int gDrawAabb; int gWireFrame; int gHelpText; + int gDebugConstraints; int gDebugContacts; int gDrawTextures=1; int gDrawShadows=0; @@ -80,6 +81,14 @@ void setDefaultSettings() gDrawAabb=0; gWireFrame=0; gDebugContacts=0; + //enable constraint debug visualization for first demo, only if user hasn't overridden the setting + if (testSelection>1) + { + gDebugConstraints=0; + } else + { + gDebugConstraints=1; + } gHelpText = 0; gDrawTextures=1; gDrawShadows=0; @@ -118,6 +127,14 @@ void ResetScene() void NextScene() { testSelection++; + if (testSelection>1) + { + gDebugConstraints=0; + } else + { + gDebugConstraints=1; + } + if(testSelection>23) testSelection=0; if (glui) @@ -216,6 +233,13 @@ void SimulationLoop() { demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_NoHelpText); } + if (gDebugConstraints) + { + demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits); + } else + { + demo->setDebugMode(demo->getDebugMode() & (~btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits)); + } if (gDebugContacts) { demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawContactPoints); @@ -281,6 +305,14 @@ void SimulationLoop() } if (testSelection != testIndex) { + if (testSelection>1) + { + gDebugConstraints=0; + } else + { + gDebugConstraints=1; + } + testIndex = testSelection; if (demo->getDynamicsWorld() && demo->getDynamicsWorld()->getDebugDrawer()) delete demo->getDynamicsWorld()->getDebugDrawer(); @@ -464,6 +496,7 @@ int main(int argc, char** argv) glui->add_checkbox_to_panel(drawPanel, "AABBs", &gDrawAabb); glui->add_checkbox_to_panel(drawPanel, "Wireframe", &gWireFrame); glui->add_checkbox_to_panel(drawPanel, "Contacts", &gDebugContacts); + glui->add_checkbox_to_panel(drawPanel, "Constraints", &gDebugConstraints); glui->add_checkbox_to_panel(drawPanel, "Textures", &gDrawTextures); glui->add_checkbox_to_panel(drawPanel, "Shadows", &gDrawShadows); diff --git a/Demos/ColladaDemo/ColladaDemo.cpp b/Demos/ColladaDemo/ColladaDemo.cpp index 7e3503338..88f2d50a2 100644 --- a/Demos/ColladaDemo/ColladaDemo.cpp +++ b/Demos/ColladaDemo/ColladaDemo.cpp @@ -76,7 +76,7 @@ int main(int argc,char** argv) /// Import Collada 1.4 Physics objects /// also can pass filename in as argument - const char* filename = "boxc4d.dae"; + const char* filename = "jenga.dae"; printf("argc=%i\n",argc); { for (int i=0;isetLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); m_joints[JOINT_PELVIS_SPINE] = hingeC; + hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); @@ -206,6 +210,8 @@ public: coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); m_joints[JOINT_SPINE_HEAD] = coneC; + coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); @@ -215,6 +221,8 @@ public: coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_LEFT_HIP] = coneC; + coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); localA.setIdentity(); localB.setIdentity(); @@ -223,6 +231,8 @@ public: hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_KNEE] = hingeC; + hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); @@ -232,6 +242,8 @@ public: coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); coneC->setLimit(M_PI_4, M_PI_4, 0); m_joints[JOINT_RIGHT_HIP] = coneC; + coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); localA.setIdentity(); localB.setIdentity(); @@ -240,6 +252,8 @@ public: hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_KNEE] = hingeC; + hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); @@ -248,6 +262,8 @@ public: localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); + coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_joints[JOINT_LEFT_SHOULDER] = coneC; m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); @@ -258,6 +274,8 @@ public: // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_LEFT_ELBOW] = hingeC; + hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); @@ -268,6 +286,8 @@ public: coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); coneC->setLimit(M_PI_2, M_PI_2, 0); m_joints[JOINT_RIGHT_SHOULDER] = coneC; + coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); localA.setIdentity(); localB.setIdentity(); @@ -277,6 +297,8 @@ public: // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); m_joints[JOINT_RIGHT_ELBOW] = hingeC; + hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE); + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); } diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 2ef83afe5..a6e36b470 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -1,1295 +1,1295 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btDbvt implementation by Nathanael Presson - -#include "btDbvt.h" - -// -typedef btAlignedObjectArray tNodeArray; -typedef btAlignedObjectArray tConstNodeArray; - -// -struct btDbvtNodeEnumerator : btDbvt::ICollide -{ - tConstNodeArray nodes; - void Process(const btDbvtNode* n) { nodes.push_back(n); } -}; - -// -static DBVT_INLINE int indexof(const btDbvtNode* node) -{ - return(node->parent->childs[1]==node); -} - -// -static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, - const btDbvtVolume& b) -{ -#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); - btDbvtVolume& res=*(btDbvtVolume*)locals; -#else - btDbvtVolume res; -#endif - Merge(a,b,res); - return(res); -} - -// volume+edge lengths -static DBVT_INLINE btScalar size(const btDbvtVolume& a) -{ - const btVector3 edges=a.Lengths(); - return( edges.x()*edges.y()*edges.z()+ - edges.x()+edges.y()+edges.z()); -} - -// -static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) -{ - if(node->isinternal()) - { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[0],depth+1,maxdepth); - } else maxdepth=btMax(maxdepth,depth); -} - -// -static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) -{ - btAlignedFree(pdbvt->m_free); - pdbvt->m_free=node; -} - -// -static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) -{ - if(!node->isleaf()) - { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); - } - if(node==pdbvt->m_root) pdbvt->m_root=0; - deletenode(pdbvt,node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) -{ - btDbvtNode* node; - if(pdbvt->m_free) - { node=pdbvt->m_free;pdbvt->m_free=0; } - else - { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } - node->parent = parent; - node->data = data; - node->childs[1] = 0; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - node->volume=volume; - return(node); -} - -// -static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume0, - const btDbvtVolume& volume1, - void* data) -{ - btDbvtNode* node=createnode(pdbvt,parent,data); - Merge(volume0,volume1,node->volume); - return(node); -} - -// -static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) -{ - if(!pdbvt->m_root) - { - pdbvt->m_root = leaf; - leaf->parent = 0; - } - else - { - if(!root->isleaf()) - { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; - } while(!root->isleaf()); - } - btDbvtNode* prev=root->parent; - btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); - if(prev) - { - prev->childs[indexof(root)] = node; - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - do { - if(!prev->volume.Contain(node->volume)) - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - else - break; - node=prev; - } while(0!=(prev=node->parent)); - } - else - { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; - } - } -} - -// -static btDbvtNode* removeleaf( btDbvt* pdbvt, - btDbvtNode* leaf) -{ - if(leaf==pdbvt->m_root) - { - pdbvt->m_root=0; - return(0); - } - else - { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) - { - prev->childs[indexof(parent)]=sibling; - sibling->parent=prev; - deletenode(pdbvt,parent); - while(prev) - { - const btDbvtVolume pb=prev->volume; - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - if(NotEqual(pb,prev->volume)) - { - prev=prev->parent; - } else break; - } - return(prev?prev:pdbvt->m_root); - } - else - { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); - } - } -} - -// -static void fetchleaves(btDbvt* pdbvt, - btDbvtNode* root, - tNodeArray& leaves, - int depth=-1) -{ - if(root->isinternal()&&depth) - { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); - } - else - { - leaves.push_back(root); - } -} - -// -static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) -{ - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); - } -} - -// -static btDbvtVolume bounds( const tNodeArray& leaves) -{ -#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE - ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); - btDbvtVolume& volume=*(btDbvtVolume*)locals; - volume=leaves[0]->volume; -#else - btDbvtVolume volume=leaves[0]->volume; -#endif - for(int i=1,ni=leaves.size();ivolume,volume); - } - return(volume); -} - -// -static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) -{ - while(leaves.size()>1) - { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); - if(szvolume,n[1]->volume,0); - p->childs[0] = n[0]; - p->childs[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); - } -} - -// -static btDbvtNode* topdown(btDbvt* pdbvt, - tNodeArray& leaves, - int bu_treshold) -{ - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - if(leaves.size()>1) - { - if(leaves.size()>bu_treshold) - { - const btDbvtVolume vol=bounds(leaves); - const btVector3 org=vol.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leaves.size(); - int splitcount[3][2]={{0,0},{0,0},{0,0}}; - int i; - for( i=0;ivolume.Center()-org; - for(int j=0;j<3;++j) - { - ++splitcount[j][dot(x,axis[j])>0?1:0]; - } - } - for( i=0;i<3;++i) - { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) - { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp=0) - { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); - } - else - { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); - node->childs[0]->parent=node; - node->childs[1]->parent=node; - return(node); - } - else - { - bottomup(pdbvt,leaves); - return(leaves[0]); - } - } - return(leaves[0]); -} - -// -static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) -{ - btDbvtNode* p=n->parent; - btAssert(n->isinternal()); - if(p>n) - { - const int i=indexof(n); - const int j=1-i; - btDbvtNode* s=p->childs[j]; - btDbvtNode* q=p->parent; - btAssert(n==p->childs[i]); - if(q) q->childs[indexof(p)]=n; else r=n; - s->parent=n; - p->parent=n; - n->parent=q; - p->childs[0]=n->childs[0]; - p->childs[1]=n->childs[1]; - n->childs[0]->parent=p; - n->childs[1]->parent=p; - n->childs[i]=p; - n->childs[j]=s; - btSwap(p->volume,n->volume); - return(p); - } - return(n); -} - -#if 0 -static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) -{ - while(n&&(count--)) n=n->parent; - return(n); -} -#endif - -// -// Api -// - -// -btDbvt::btDbvt() -{ - m_root = 0; - m_free = 0; - m_lkhd = -1; - m_leaves = 0; - m_opath = 0; -} - -// -btDbvt::~btDbvt() -{ - clear(); -} - -// -void btDbvt::clear() -{ - if(m_root) - recursedeletenode(this,m_root); - btAlignedFree(m_free); - m_free=0; - m_lkhd = -1; - m_stkStack.clear(); - m_opath = 0; - -} - -// -void btDbvt::optimizeBottomUp() -{ - if(m_root) - { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,leaves); - m_root=leaves[0]; - } -} - -// -void btDbvt::optimizeTopDown(int bu_treshold) -{ - if(m_root) - { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); - } -} - -// -void btDbvt::optimizeIncremental(int passes) -{ - if(passes<0) passes=m_leaves; - if(m_root&&(passes>0)) - { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) - { - node=sort(node,m_root)->childs[(m_opath>>bit)&1]; - bit=(bit+1)&(sizeof(unsigned)*8-1); - } - update(node); - ++m_opath; - } while(--passes); - } -} - -// -btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) -{ - btDbvtNode* leaf=createnode(this,0,volume,data); - insertleaf(this,m_root,leaf); - ++m_leaves; - return(leaf); -} - -// -void btDbvt::update(btDbvtNode* leaf,int lookahead) -{ - btDbvtNode* root=removeleaf(this,leaf); - if(root) - { - if(lookahead>=0) - { - for(int i=0;(iparent;++i) - { - root=root->parent; - } - } else root=m_root; - } - insertleaf(this,root,leaf); -} - -// -void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) -{ - btDbvtNode* root=removeleaf(this,leaf); - if(root) - { - if(m_lkhd>=0) - { - for(int i=0;(iparent;++i) - { - root=root->parent; - } - } else root=m_root; - } - leaf->volume=volume; - insertleaf(this,root,leaf); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.SignedExpand(velocity); - update(leaf,volume); - return(true); -} - -// -bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) -{ - if(leaf->volume.Contain(volume)) return(false); - volume.Expand(btVector3(margin,margin,margin)); - update(leaf,volume); - return(true); -} - -// -void btDbvt::remove(btDbvtNode* leaf) -{ - removeleaf(this,leaf); - deletenode(this,leaf); - --m_leaves; -} - -// -void btDbvt::write(IWriter* iwriter) const -{ - btDbvtNodeEnumerator nodes; - nodes.nodes.reserve(m_leaves*2); - enumNodes(m_root,nodes); - iwriter->Prepare(m_root,nodes.nodes.size()); - for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); - if(n->isinternal()) - { - const int c0=nodes.nodes.findLinearSearch(n->childs[0]); - const int c1=nodes.nodes.findLinearSearch(n->childs[1]); - iwriter->WriteNode(n,i,p,c0,c1); - } - else - { - iwriter->WriteLeaf(n,i,p); - } - } -} - -// -void btDbvt::clone(btDbvt& dest,IClone* iclone) const -{ - dest.clear(); - if(m_root!=0) - { - btAlignedObjectArray stack; - stack.reserve(m_leaves); - stack.push_back(sStkCLN(m_root,0)); - do { - const int i=stack.size()-1; - const sStkCLN e=stack[i]; - btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); - stack.pop_back(); - if(e.parent!=0) - e.parent->childs[i&1]=n; - else - dest.m_root=n; - if(e.node->isinternal()) - { - stack.push_back(sStkCLN(e.node->childs[0],n)); - stack.push_back(sStkCLN(e.node->childs[1],n)); - } - else - { - iclone->CloneLeaf(n); - } - } while(stack.size()>0); - } -} - -// -int btDbvt::maxdepth(const btDbvtNode* node) -{ - int depth=0; - if(node) getmaxdepth(node,1,depth); - return(depth); -} - -// -int btDbvt::countLeaves(const btDbvtNode* node) -{ - if(node->isinternal()) - return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); - else - return(1); -} - -// -void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) -{ - if(node->isinternal()) - { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); - } - else - { - leaves.push_back(node); - } -} - -// -#if DBVT_ENABLE_BENCHMARK - -#include -#include -#include "LinearMath/btQuickProf.h" - -/* -q6600,2.4ghz - -/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" -/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" -/Fo"..\..\out\release8\build\libbulletcollision\\" -/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" -/W3 /nologo /c /Wp64 /Zi /errorReport:prompt - -Benchmarking dbvt... -World scale: 100.000000 -Extents base: 1.000000 -Extents range: 4.000000 -Leaves: 8192 -sizeof(btDbvtVolume): 32 bytes -sizeof(btDbvtNode): 44 bytes -[1] btDbvtVolume intersections: 3499 ms (-1%) -[2] btDbvtVolume merges: 1934 ms (0%) -[3] btDbvt::collideTT: 5485 ms (-21%) -[4] btDbvt::collideTT self: 2814 ms (-20%) -[5] btDbvt::collideTT xform: 7379 ms (-1%) -[6] btDbvt::collideTT xform,self: 7270 ms (-2%) -[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) -[8] insert/remove: 2093 ms (0%),(1001983 ir/s) -[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) -[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) -[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) -[12] btDbvtVolume notequal: 3659 ms (0%) -[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) -[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) -[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) -[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) -[17] btDbvtVolume select: 3419 ms (0%) -*/ - -struct btDbvtBenchmark -{ - struct NilPolicy : btDbvt::ICollide - { - NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} - void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*,btScalar depth) - { - ++m_pcount; - if(m_checksort) - { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } - } - int m_pcount; - btScalar m_depth; - bool m_checksort; - }; - struct P14 : btDbvt::ICollide - { - struct Node - { - const btDbvtNode* leaf; - btScalar depth; - }; - void Process(const btDbvtNode* leaf,btScalar depth) - { - Node n; - n.leaf = leaf; - n.depth = depth; - } - static int sortfnc(const Node& a,const Node& b) - { - if(a.depthb.depth) return(-1); - return(0); - } - btAlignedObjectArray m_nodes; - }; - struct P15 : btDbvt::ICollide - { - struct Node - { - const btDbvtNode* leaf; - btScalar depth; - }; - void Process(const btDbvtNode* leaf) - { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); - } - static int sortfnc(const Node& a,const Node& b) - { - if(a.depthb.depth) return(-1); - return(0); - } - btAlignedObjectArray m_nodes; - btVector3 m_axis; - }; - static btScalar RandUnit() - { - return(rand()/(btScalar)RAND_MAX); - } - static btVector3 RandVector3() - { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); - } - static btVector3 RandVector3(btScalar cs) - { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); - } - static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) - { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); - } - static btTransform RandTransform(btScalar cs) - { - btTransform t; - t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(t); - } - static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) - { - dbvt.clear(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i rayorg; - btAlignedObjectArray raydir; - btDbvtBenchmark::NilPolicy policy; - rayorg.resize(cfgBenchmark7_Iterations); - raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); - printf("[9] updates (teleport): "); - wallclock.reset(); - for(int i=0;i(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); - } - } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); - } - if(cfgBenchmark10_Enable) - {// Benchmark 10 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray leaves; - btAlignedObjectArray vectors; - vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i(leaves[rand()%cfgLeaves]); - btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); - dbvt.update(l,v); - } - } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); - } - if(cfgBenchmark11_Enable) - {// Benchmark 11 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[11] optimize (incremental): "); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i vectors; - btDbvtBenchmark::NilPolicy policy; - vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P14 policy; - vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i vectors; - btDbvtBenchmark::P15 policy; - vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - batch.reserve(cfgBenchmark16_BatchCount); - printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - btAlignedObjectArray indices; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - indices.resize(cfgLeaves); - for(int i=0;i tNodeArray; +typedef btAlignedObjectArray tConstNodeArray; + +// +struct btDbvtNodeEnumerator : btDbvt::ICollide +{ + tConstNodeArray nodes; + void Process(const btDbvtNode* n) { nodes.push_back(n); } +}; + +// +static DBVT_INLINE int indexof(const btDbvtNode* node) +{ + return(node->parent->childs[1]==node); +} + +// +static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, + const btDbvtVolume& b) +{ +#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume& res=*(btDbvtVolume*)locals; +#else + btDbvtVolume res; +#endif + Merge(a,b,res); + return(res); +} + +// volume+edge lengths +static DBVT_INLINE btScalar size(const btDbvtVolume& a) +{ + const btVector3 edges=a.Lengths(); + return( edges.x()*edges.y()*edges.z()+ + edges.x()+edges.y()+edges.z()); +} + +// +static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) +{ + if(node->isinternal()) + { + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); + } else maxdepth=btMax(maxdepth,depth); +} + +// +static DBVT_INLINE void deletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ + btAlignedFree(pdbvt->m_free); + pdbvt->m_free=node; +} + +// +static void recursedeletenode( btDbvt* pdbvt, + btDbvtNode* node) +{ + if(!node->isleaf()) + { + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); + } + if(node==pdbvt->m_root) pdbvt->m_root=0; + deletenode(pdbvt,node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + void* data) +{ + btDbvtNode* node; + if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) +{ + btDbvtNode* node=createnode(pdbvt,parent,data); + node->volume=volume; + return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) +{ + btDbvtNode* node=createnode(pdbvt,parent,data); + Merge(volume0,volume1,node->volume); + return(node); +} + +// +static void insertleaf( btDbvt* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) +{ + if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + btDbvtNode* prev=root->parent; + btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + else + break; + node=prev; + } while(0!=(prev=node->parent)); + } + else + { + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; + } + } +} + +// +static btDbvtNode* removeleaf( btDbvt* pdbvt, + btDbvtNode* leaf) +{ + if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + btDbvtNode* parent=leaf->parent; + btDbvtNode* prev=parent->parent; + btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; + if(prev) + { + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) + { + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void fetchleaves(btDbvt* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth=-1) +{ + if(root->isinternal()&&depth) + { + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +// +static void split( const tNodeArray& leaves, + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) +{ + left.resize(0); + right.resize(0); + for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) + left.push_back(leaves[i]); + else + right.push_back(leaves[i]); + } +} + +// +static btDbvtVolume bounds( const tNodeArray& leaves) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume& volume=*(btDbvtVolume*)locals; + volume=leaves[0]->volume; +#else + btDbvtVolume volume=leaves[0]->volume; +#endif + for(int i=1,ni=leaves.size();ivolume,volume); + } + return(volume); +} + +// +static void bottomup( btDbvt* pdbvt, + tNodeArray& leaves) +{ + while(leaves.size()>1) + { + btScalar minsize=SIMD_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;ivolume,leaves[j]->volume)); + if(szvolume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); + } +} + +// +static btDbvtNode* topdown(btDbvt* pdbvt, + tNodeArray& leaves, + int bu_treshold) +{ + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + if(leaves.size()>1) + { + if(leaves.size()>bu_treshold) + { + const btDbvtVolume vol=bounds(leaves); + const btVector3 org=vol.Center(); + tNodeArray sets[2]; + int bestaxis=-1; + int bestmidp=leaves.size(); + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;ivolume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][dot(x,axis[j])>0?1:0]; + } + } + for( i=0;i<3;++i) + { + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + { + const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); + if(midp=0) + { + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); + } + else + { + sets[0].reserve(leaves.size()/2+1); + sets[1].reserve(leaves.size()/2); + for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + bottomup(pdbvt,leaves); + return(leaves[0]); + } + } + return(leaves[0]); +} + +// +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +{ + btDbvtNode* p=n->parent; + btAssert(n->isinternal()); + if(p>n) + { + const int i=indexof(n); + const int j=1-i; + btDbvtNode* s=p->childs[j]; + btDbvtNode* q=p->parent; + btAssert(n==p->childs[i]); + if(q) q->childs[indexof(p)]=n; else r=n; + s->parent=n; + p->parent=n; + n->parent=q; + p->childs[0]=n->childs[0]; + p->childs[1]=n->childs[1]; + n->childs[0]->parent=p; + n->childs[1]->parent=p; + n->childs[i]=p; + n->childs[j]=s; + btSwap(p->volume,n->volume); + return(p); + } + return(n); +} + +#if 0 +static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) +{ + while(n&&(count--)) n=n->parent; + return(n); +} +#endif + +// +// Api +// + +// +btDbvt::btDbvt() +{ + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; +} + +// +btDbvt::~btDbvt() +{ + clear(); +} + +// +void btDbvt::clear() +{ + if(m_root) + recursedeletenode(this,m_root); + btAlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + +} + +// +void btDbvt::optimizeBottomUp() +{ + if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; + } +} + +// +void btDbvt::optimizeTopDown(int bu_treshold) +{ + if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); + } +} + +// +void btDbvt::optimizeIncremental(int passes) +{ + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) + { + do { + btDbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) +{ + btDbvtNode* leaf=createnode(this,0,volume,data); + insertleaf(this,m_root,leaf); + ++m_leaves; + return(leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,int lookahead) +{ + btDbvtNode* root=removeleaf(this,leaf); + if(root) + { + if(lookahead>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + insertleaf(this,root,leaf); +} + +// +void btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume) +{ + btDbvtNode* root=removeleaf(this,leaf); + if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + leaf->volume=volume; + insertleaf(this,root,leaf); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(btVector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(btVector3(margin,margin,margin)); + update(leaf,volume); + return(true); +} + +// +void btDbvt::remove(btDbvtNode* leaf) +{ + removeleaf(this,leaf); + deletenode(this,leaf); + --m_leaves; +} + +// +void btDbvt::write(IWriter* iwriter) const +{ + btDbvtNodeEnumerator nodes; + nodes.nodes.reserve(m_leaves*2); + enumNodes(m_root,nodes); + iwriter->Prepare(m_root,nodes.nodes.size()); + for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) + { + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); + } + else + { + iwriter->WriteLeaf(n,i,p); + } + } +} + +// +void btDbvt::clone(btDbvt& dest,IClone* iclone) const +{ + dest.clear(); + if(m_root!=0) + { + btAlignedObjectArray stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; + else + dest.m_root=n; + if(e.node->isinternal()) + { + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); + } + else + { + iclone->CloneLeaf(n); + } + } while(stack.size()>0); + } +} + +// +int btDbvt::maxdepth(const btDbvtNode* node) +{ + int depth=0; + if(node) getmaxdepth(node,1,depth); + return(depth); +} + +// +int btDbvt::countLeaves(const btDbvtNode* node) +{ + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) +{ + if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if DBVT_ENABLE_BENCHMARK + +#include +#include +#include "LinearMath/btQuickProf.h" + +/* +q6600,2.4ghz + +/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" +/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" +/Fo"..\..\out\release8\build\libbulletcollision\\" +/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" +/W3 /nologo /c /Wp64 /Zi /errorReport:prompt + +Benchmarking dbvt... +World scale: 100.000000 +Extents base: 1.000000 +Extents range: 4.000000 +Leaves: 8192 +sizeof(btDbvtVolume): 32 bytes +sizeof(btDbvtNode): 44 bytes +[1] btDbvtVolume intersections: 3499 ms (-1%) +[2] btDbvtVolume merges: 1934 ms (0%) +[3] btDbvt::collideTT: 5485 ms (-21%) +[4] btDbvt::collideTT self: 2814 ms (-20%) +[5] btDbvt::collideTT xform: 7379 ms (-1%) +[6] btDbvt::collideTT xform,self: 7270 ms (-2%) +[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s) +[8] insert/remove: 2093 ms (0%),(1001983 ir/s) +[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) +[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) +[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) +[12] btDbvtVolume notequal: 3659 ms (0%) +[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) +[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) +[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) +[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) +[17] btDbvtVolume select: 3419 ms (0%) +*/ + +struct btDbvtBenchmark +{ + struct NilPolicy : btDbvt::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} + void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*,btScalar depth) + { + ++m_pcount; + if(m_checksort) + { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } + } + int m_pcount; + btScalar m_depth; + bool m_checksort; + }; + struct P14 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf,btScalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + btAlignedObjectArray m_nodes; + }; + struct P15 : btDbvt::ICollide + { + struct Node + { + const btDbvtNode* leaf; + btScalar depth; + }; + void Process(const btDbvtNode* leaf) + { + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + btAlignedObjectArray m_nodes; + btVector3 m_axis; + }; + static btScalar RandUnit() + { + return(rand()/(btScalar)RAND_MAX); + } + static btVector3 RandVector3() + { + return(btVector3(RandUnit(),RandUnit(),RandUnit())); + } + static btVector3 RandVector3(btScalar cs) + { + return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + } + static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + { + return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + } + static btTransform RandTransform(btScalar cs) + { + btTransform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); + return(t); + } + static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) + { + dbvt.clear(); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; + btAlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i(leaves[rand()%cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + } + if(cfgBenchmark10_Enable) + {// Benchmark 10 + srand(380843); + btDbvt dbvt; + btAlignedObjectArray leaves; + btAlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); + btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); + dbvt.update(l,v); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + } + if(cfgBenchmark11_Enable) + {// Benchmark 11 + srand(380843); + btDbvt dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i volumes; + btAlignedObjectArray results; + btAlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) -#define DBVT_USE_TEMPLATE 1 -#else -#define DBVT_USE_TEMPLATE 0 -#endif -#else -#define DBVT_USE_TEMPLATE 0 -#endif - -// Use only intrinsics instead of inline asm -#define DBVT_USE_INTRINSIC_SSE 1 - -// Using memmov for collideOCL -#define DBVT_USE_MEMMOVE 1 - -// Enable benchmarking code -#define DBVT_ENABLE_BENCHMARK 0 - -// Inlining -#define DBVT_INLINE SIMD_FORCE_INLINE - -// Specific methods implementation - -//SSE gives errors on a MSVC 7.1 -#ifdef BT_USE_SSE -#define DBVT_SELECT_IMPL DBVT_IMPL_SSE -#define DBVT_MERGE_IMPL DBVT_IMPL_SSE -#define DBVT_INT0_IMPL DBVT_IMPL_SSE -#else -#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC -#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC -#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC -#endif - -#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ - (DBVT_INT0_IMPL==DBVT_IMPL_SSE) -#include -#endif - -// -// Auto config and checks -// - -#if DBVT_USE_TEMPLATE -#define DBVT_VIRTUAL -#define DBVT_VIRTUAL_DTOR(a) -#define DBVT_PREFIX template -#define DBVT_IPOLICY T& policy -#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; -#else -#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} -#define DBVT_VIRTUAL virtual -#define DBVT_PREFIX -#define DBVT_IPOLICY ICollide& policy -#define DBVT_CHECKTYPE -#endif - -#if DBVT_USE_MEMMOVE -#ifndef __CELLOS_LV2__ -#include -#endif -#include -#endif - -#ifndef DBVT_USE_TEMPLATE -#error "DBVT_USE_TEMPLATE undefined" -#endif - -#ifndef DBVT_USE_MEMMOVE -#error "DBVT_USE_MEMMOVE undefined" -#endif - -#ifndef DBVT_ENABLE_BENCHMARK -#error "DBVT_ENABLE_BENCHMARK undefined" -#endif - -#ifndef DBVT_SELECT_IMPL -#error "DBVT_SELECT_IMPL undefined" -#endif - -#ifndef DBVT_MERGE_IMPL -#error "DBVT_MERGE_IMPL undefined" -#endif - -#ifndef DBVT_INT0_IMPL -#error "DBVT_INT0_IMPL undefined" -#endif - -// -// Defaults volumes -// - -/* btDbvtAabbMm */ -struct btDbvtAabbMm -{ - DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } - DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } - DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } - DBVT_INLINE const btVector3& Mins() const { return(mi); } - DBVT_INLINE const btVector3& Maxs() const { return(mx); } - static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); - static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); - static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); - static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); - static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); - DBVT_INLINE void Expand(const btVector3& e); - DBVT_INLINE void SignedExpand(const btVector3& e); - DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; - DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; - DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - - DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); - - DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b); - DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); - DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -private: - DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; -private: - btVector3 mi,mx; -}; - -// Types -typedef btDbvtAabbMm btDbvtVolume; - -/* btDbvtNode */ -struct btDbvtNode -{ - btDbvtVolume volume; - btDbvtNode* parent; - DBVT_INLINE bool isleaf() const { return(childs[1]==0); } - DBVT_INLINE bool isinternal() const { return(!isleaf()); } - union - { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; - }; -}; - -///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). -///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. -///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. -struct btDbvt -{ - /* Stack element */ - struct sStkNN - { - const btDbvtNode* a; - const btDbvtNode* b; - sStkNN() {} - sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} - }; - struct sStkNP - { - const btDbvtNode* node; - int mask; - sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} - }; - struct sStkNPS - { - const btDbvtNode* node; - int mask; - btScalar value; - sStkNPS() {} - sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} - }; - struct sStkCLN - { - const btDbvtNode* node; - btDbvtNode* parent; - sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} - }; - // Policies/Interfaces - - /* ICollide */ - struct ICollide - { - DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode*) {} - DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } - DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } - DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } - }; - /* IWriter */ - struct IWriter - { - virtual ~IWriter() {} - virtual void Prepare(const btDbvtNode* root,int numnodes)=0; - virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; - virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; - }; - /* IClone */ - struct IClone - { - virtual ~IClone() {} - virtual void CloneLeaf(btDbvtNode*) {} - }; - - // Constants - enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 - }; - - // Fields - btDbvtNode* m_root; - btDbvtNode* m_free; - int m_lkhd; - int m_leaves; - unsigned m_opath; - - - btAlignedObjectArray m_stkStack; - - - // Methods - btDbvt(); - ~btDbvt(); - void clear(); - bool empty() const { return(0==m_root); } - void optimizeBottomUp(); - void optimizeTopDown(int bu_treshold=128); - void optimizeIncremental(int passes); - btDbvtNode* insert(const btDbvtVolume& box,void* data); - void update(btDbvtNode* leaf,int lookahead=-1); - void update(btDbvtNode* leaf,btDbvtVolume& volume); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity); - bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); - void remove(btDbvtNode* leaf); - void write(IWriter* iwriter) const; - void clone(btDbvt& dest,IClone* iclone=0) const; - static int maxdepth(const btDbvtNode* node); - static int countLeaves(const btDbvtNode* node); - static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves); -#if DBVT_ENABLE_BENCHMARK - static void benchmark(); -#else - static void benchmark(){} -#endif - // DBVT_IPOLICY must support ICollide policy/interface - DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); - DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); - - DBVT_PREFIX - void collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); -#if 0 - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY); - DBVT_PREFIX - void collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY); -#endif - - DBVT_PREFIX - void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY); - ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) - ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time - DBVT_PREFIX - static void rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY); - ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections - ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts - DBVT_PREFIX - void rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - DBVT_IPOLICY) const; - - DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); - DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); - DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); - // Helpers - static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) - { - int m=0; - while(l>1; - if(a[i[m]].value>=v) l=m+1; else h=m; - } - return(h); - } - static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, - btAlignedObjectArray& stock, - const sStkNPS& value) - { - int i; - if(ifree.size()>0) - { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } - else - { i=stock.size();stock.push_back(value); } - return(i); - } - // -private: - btDbvt(const btDbvt&) {} -}; - -// -// Inline's -// - -// -inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) -{ - btDbvtAabbMm box; - box.mi=c-e;box.mx=c+e; - return(box); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) -{ - return(FromCE(c,btVector3(r,r,r))); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) -{ - btDbvtAabbMm box; - box.mi=mi;box.mx=mx; - return(box); -} - -// -inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) -{ - btDbvtAabbMm box; - box.mi=box.mx=pts[0]; - for(int i=1;i0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); - if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); - if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); -} - -// -DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const -{ - return( (mi.x()<=a.mi.x())&& - (mi.y()<=a.mi.y())&& - (mi.z()<=a.mi.z())&& - (mx.x()>=a.mx.x())&& - (mx.y()>=a.mx.y())&& - (mx.z()>=a.mx.z())); -} - -// -DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const -{ - btVector3 pi,px; - switch(s) - { - case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); - pi=btVector3(mx.x(),mx.y(),mx.z());break; - case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); - pi=btVector3(mi.x(),mx.y(),mx.z());break; - case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); - pi=btVector3(mx.x(),mi.y(),mx.z());break; - case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); - pi=btVector3(mi.x(),mi.y(),mx.z());break; - case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); - pi=btVector3(mx.x(),mx.y(),mi.z());break; - case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); - pi=btVector3(mi.x(),mx.y(),mi.z());break; - case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); - pi=btVector3(mx.x(),mi.y(),mi.z());break; - case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); - pi=btVector3(mi.x(),mi.y(),mi.z());break; - } - if((dot(n,px)+o)<0) return(-1); - if((dot(n,pi)+o)>=0) return(+1); - return(0); -} - -// -DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const -{ - const btVector3* b[]={&mx,&mi}; - const btVector3 p( b[(signs>>0)&1]->x(), - b[(signs>>1)&1]->y(), - b[(signs>>2)&1]->z()); - return(dot(p,v)); -} - -// -DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const -{ - for(int i=0;i<3;++i) - { - if(d[i]<0) - { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } - else - { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } - } -} - -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ -#if DBVT_INT0_IMPL == DBVT_IMPL_SSE - const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); - const __int32* pu((const __int32*)&rt); - return((pu[0]|pu[1]|pu[2])==0); -#else - return( (a.mi.x()<=b.mx.x())&& - (a.mx.x()>=b.mi.x())&& - (a.mi.y()<=b.mx.y())&& - (a.mx.y()>=b.mi.y())&& - (a.mi.z()<=b.mx.z())&& - (a.mx.z()>=b.mi.z())); -#endif -} - - - -// -DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) -{ - return( (b.x()>=a.mi.x())&& - (b.y()>=a.mi.y())&& - (b.z()>=a.mi.z())&& - (b.x()<=a.mx.x())&& - (b.y()<=a.mx.y())&& - (b.z()<=a.mx.z())); -} - - - - - -////////////////////////////////////// - - -// -DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ - const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); - return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); -} - - - -// -DBVT_INLINE int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ -#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE - static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; - ///@todo: the intrinsic version is 11% slower -#if DBVT_USE_INTRINSIC_SSE - - union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory - { - __m128 ssereg; - float floats[4]; - int ints[4]; - }; - - __m128 omi(_mm_load_ps(o.mi)); - omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); - __m128 ami(_mm_load_ps(a.mi)); - ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); - ami=_mm_sub_ps(ami,omi); - ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); - __m128 bmi(_mm_load_ps(b.mi)); - bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); - bmi=_mm_sub_ps(bmi,omi); - bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); - __m128 t0(_mm_movehl_ps(ami,ami)); - ami=_mm_add_ps(ami,t0); - ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); - __m128 t1(_mm_movehl_ps(bmi,bmi)); - bmi=_mm_add_ps(bmi,t1); - bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); - - btSSEUnion tmp; - tmp.ssereg = _mm_cmple_ss(bmi,ami); - return tmp.ints[0]&1; - -#else - ATTRIBUTE_ALIGNED16(__int32 r[1]); - __asm - { - mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] - movaps xmm5,mask - addps xmm0,[eax+16] - movaps xmm1,[ecx] - movaps xmm2,[edx] - addps xmm1,[ecx+16] - addps xmm2,[edx+16] - subps xmm1,xmm0 - subps xmm2,xmm0 - andps xmm1,xmm5 - andps xmm2,xmm5 - movhlps xmm3,xmm1 - movhlps xmm4,xmm2 - addps xmm1,xmm3 - addps xmm2,xmm4 - pshufd xmm3,xmm1,1 - pshufd xmm4,xmm2,1 - addss xmm1,xmm3 - addss xmm2,xmm4 - cmpless xmm2,xmm1 - movss r,xmm2 - } - return(r[0]&1); -#endif -#else - return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; - } -#endif -} - -// -DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) -{ - return( (a.mi.x()!=b.mi.x())|| - (a.mi.y()!=b.mi.y())|| - (a.mi.z()!=b.mi.z())|| - (a.mx.x()!=b.mx.x())|| - (a.mx.y()!=b.mx.y())|| - (a.mx.z()!=b.mx.z())); -} - -// -// Inline's -// - -// -DBVT_PREFIX -inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - policy.Process(root); - if(root->isinternal()) - { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); - } -} - -// -DBVT_PREFIX -inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(depth>treshold) - { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; - } - if(p.a==p.b) - { - if(p.a->isinternal()) - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); - } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } - } - else - { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } - } - } - } while(depth); - } -} - - - -DBVT_PREFIX -inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - - m_stkStack.resize(DOUBLE_STACKSIZE); - m_stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=m_stkStack[--depth]; - if(depth>treshold) - { - m_stkStack.resize(m_stkStack.size()*2); - treshold=m_stkStack.size()-4; - } - if(p.a==p.b) - { - if(p.a->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); - } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } - } - else - { - if(p.b->isinternal()) - { - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } - } - } - } while(depth); - } -} - -#if 0 -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root0&&root1) - { - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - btAlignedObjectArray stkStack; - stkStack.resize(DOUBLE_STACKSIZE); - stkStack[0]=sStkNN(root0,root1); - do { - sStkNN p=stkStack[--depth]; - if(Intersect(p.a->volume,p.b->volume,xform)) - { - if(depth>treshold) - { - stkStack.resize(stkStack.size()*2); - treshold=stkStack.size()-4; - } - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); - } - else - { - stkStack[depth++]=sStkNN(p.a->childs[0],p.b); - stkStack[depth++]=sStkNN(p.a->childs[1],p.b); - } - } - else - { - if(p.b->isinternal()) - { - stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); - stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } - } - } - } while(depth); - } -} -// -DBVT_PREFIX -inline void btDbvt::collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY) -{ - const btTransform xform=xform0.inverse()*xform1; - collideTT(root0,root1,xform,policy); -} -#endif - -// -DBVT_PREFIX -inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) - { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; - stack.resize(0); - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) - { - if(n->isinternal()) - { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); - } - else - { - policy.Process(n); - } - } - } while(stack.size()>0); - } -} - -DBVT_PREFIX -inline void btDbvt::rayTestInternal( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayDirectionInverse, - unsigned int signs[3], - btScalar lambda_max, - const btVector3& aabbMin, - const btVector3& aabbMax, - DBVT_IPOLICY) const -{ - DBVT_CHECKTYPE - if(root) - { - btVector3 resultNormal; - - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; - btAlignedObjectArray stack; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; - btVector3 bounds[2]; - do - { - const btDbvtNode* node=stack[--depth]; - bounds[0] = node->volume.Mins()+aabbMin; - bounds[1] = node->volume.Maxs()+aabbMax; - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1=false; - result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - if(result1) - { - if(node->isinternal()) - { - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-2; - } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; - } - else - { - policy.Process(node); - } - } - } while(depth); - } -} - -// -DBVT_PREFIX -inline void btDbvt::rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) - { - btVector3 rayDir = (rayTo-rayFrom); - rayDir.normalize (); - - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - - btScalar lambda_max = rayDir.dot(rayTo-rayFrom); - - btVector3 resultNormal; - - btAlignedObjectArray stack; - - int depth=1; - int treshold=DOUBLE_STACKSIZE-2; - - stack.resize(DOUBLE_STACKSIZE); - stack[0]=root; - btVector3 bounds[2]; - do { - const btDbvtNode* node=stack[--depth]; - - bounds[0] = node->volume.Mins(); - bounds[1] = node->volume.Maxs(); - - btScalar tmin=1.f,lambda_min=0.f; - unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); - -#ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); -#endif //TEST_BTRAY_AABB2 - - if(result1) - { - if(node->isinternal()) - { - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-2; - } - stack[depth++]=node->childs[0]; - stack[depth++]=node->childs[1]; - } - else - { - policy.Process(node); - } - } - } while(depth); - - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) - { - const int inside=(1< stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } - } - } - if(!out) - { - if((se.mask!=inside)&&(se.node->isinternal())) - { - stack.push_back(sStkNP(se.node->childs[0],se.mask)); - stack.push_back(sStkNP(se.node->childs[1],se.mask)); - } - else - { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); - } - } - } while(stack.size()); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fsort) -{ - DBVT_CHECKTYPE - if(root) - { - const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ - (sortaxis[1]>=0?2:0)+ - (sortaxis[2]>=0?4:0); - const int inside=(1< stock; - btAlignedObjectArray ifree; - btAlignedObjectArray stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stock.reserve(SIMPLE_STACKSIZE); - stack.reserve(SIMPLE_STACKSIZE); - ifree.reserve(SIMPLE_STACKSIZE); - stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); - do { - const int id=stack[stack.size()-1]; - sStkNPS se=stock[id]; - stack.pop_back();ifree.push_back(id); - if(se.mask!=inside) - { - bool out=false; - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } - } - } - if(out) continue; - } - if(policy.Descent(se.node)) - { - if(se.node->isinternal()) - { - const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; - sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), - sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; - const int q=nes[0].value0)) - { - /* Insert 0 */ - j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); - stack.push_back(0); -#if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); -#else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; -#endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); -#if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); -#else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; -#endif - stack[j]=allocate(ifree,stock,nes[1-q]); - } - else - { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); - } - } - else - { - policy.Process(se.node,se.value); - } - } - } while(stack.size()); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) -{ - DBVT_CHECKTYPE - if(root) - { - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } - } - } while(stack.size()>0); - } -} - -// -// PP Cleanup -// - -#undef DBVT_USE_MEMMOVE -#undef DBVT_USE_TEMPLATE -#undef DBVT_VIRTUAL_DTOR -#undef DBVT_VIRTUAL -#undef DBVT_PREFIX -#undef DBVT_IPOLICY -#undef DBVT_CHECKTYPE -#undef DBVT_IMPL_GENERIC -#undef DBVT_IMPL_SSE -#undef DBVT_USE_INTRINSIC_SSE -#undef DBVT_SELECT_IMPL -#undef DBVT_MERGE_IMPL -#undef DBVT_INT0_IMPL - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvt implementation by Nathanael Presson + +#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H +#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btAabbUtil2.h" + +// +// Compile time configuration +// + + +// Implementation profiles +#define DBVT_IMPL_GENERIC 0 // Generic implementation +#define DBVT_IMPL_SSE 1 // SSE + +// Template implementation of ICollide +#ifdef WIN32 +#if (defined (_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 +#else +#define DBVT_USE_TEMPLATE 0 +#endif +#else +#define DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define DBVT_INLINE SIMD_FORCE_INLINE + +// Specific methods implementation + +//SSE gives errors on a MSVC 7.1 +#ifdef BT_USE_SSE +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#else +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#endif + +#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#include +#endif + +// +// Auto config and checks +// + +#if DBVT_USE_TEMPLATE +#define DBVT_VIRTUAL +#define DBVT_VIRTUAL_DTOR(a) +#define DBVT_PREFIX template +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#else +#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define DBVT_VIRTUAL virtual +#define DBVT_PREFIX +#define DBVT_IPOLICY ICollide& policy +#define DBVT_CHECKTYPE +#endif + +#if DBVT_USE_MEMMOVE +#ifndef __CELLOS_LV2__ +#include +#endif +#include +#endif + +#ifndef DBVT_USE_TEMPLATE +#error "DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef DBVT_USE_MEMMOVE +#error "DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef DBVT_ENABLE_BENCHMARK +#error "DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef DBVT_SELECT_IMPL +#error "DBVT_SELECT_IMPL undefined" +#endif + +#ifndef DBVT_MERGE_IMPL +#error "DBVT_MERGE_IMPL undefined" +#endif + +#ifndef DBVT_INT0_IMPL +#error "DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* btDbvtAabbMm */ +struct btDbvtAabbMm +{ + DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } + DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } + DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } + DBVT_INLINE const btVector3& Mins() const { return(mi); } + DBVT_INLINE const btVector3& Maxs() const { return(mx); } + static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); + static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); + static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); + static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); + static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); + DBVT_INLINE void Expand(const btVector3& e); + DBVT_INLINE void SignedExpand(const btVector3& e); + DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; + DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; + DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& b); + + DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); + DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); +private: + DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; +private: + btVector3 mi,mx; +}; + +// Types +typedef btDbvtAabbMm btDbvtVolume; + +/* btDbvtNode */ +struct btDbvtNode +{ + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union + { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct btDbvt +{ + /* Stack element */ + struct sStkNN + { + const btDbvtNode* a; + const btDbvtNode* b; + sStkNN() {} + sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const btDbvtNode* node; + int mask; + btScalar value; + sStkNPS() {} + sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + DBVT_VIRTUAL_DTOR(ICollide) + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const btDbvtNode* root,int numnodes)=0; + virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} + }; + + // Constants + enum { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + + // Fields + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + + + btAlignedObjectArray m_stkStack; + + + // Methods + btDbvt(); + ~btDbvt(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box,void* data); + void update(btDbvtNode* leaf,int lookahead=-1); + void update(btDbvtNode* leaf,btDbvtVolume& volume); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(btDbvt& dest,IClone* iclone=0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves); +#if DBVT_ENABLE_BENCHMARK + static void benchmark(); +#else + static void benchmark(){} +#endif + // DBVT_IPOLICY must support ICollide policy/interface + DBVT_PREFIX + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + + DBVT_PREFIX + void collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); +#if 0 + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY); + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); +#endif + + DBVT_PREFIX + void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) + ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time + DBVT_PREFIX + static void rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); + ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections + ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts + DBVT_PREFIX + void rayTestInternal( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + DBVT_IPOLICY) const; + + DBVT_PREFIX + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); + DBVT_PREFIX + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) + { + int m=0; + while(l>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, + btAlignedObjectArray& stock, + const sStkNPS& value) + { + int i; + if(ifree.size()>0) + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } + return(i); + } + // +private: + btDbvt(const btDbvt&) {} +}; + +// +// Inline's +// + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) +{ + btDbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) +{ + return(FromCE(c,btVector3(r,r,r))); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) +{ + btDbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) +{ + btDbvtAabbMm box; + box.mi=box.mx=pts[0]; + for(int i=1;i0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); + if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); + if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); +} + +// +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const +{ + return( (mi.x()<=a.mi.x())&& + (mi.y()<=a.mi.y())&& + (mi.z()<=a.mi.z())&& + (mx.x()>=a.mx.x())&& + (mx.y()>=a.mx.y())&& + (mx.z()>=a.mx.z())); +} + +// +DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const +{ + btVector3 pi,px; + switch(s) + { + case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); + pi=btVector3(mx.x(),mx.y(),mx.z());break; + case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); + pi=btVector3(mi.x(),mx.y(),mx.z());break; + case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); + pi=btVector3(mx.x(),mi.y(),mx.z());break; + case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); + pi=btVector3(mi.x(),mi.y(),mx.z());break; + case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); + pi=btVector3(mx.x(),mx.y(),mi.z());break; + case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); + pi=btVector3(mi.x(),mx.y(),mi.z());break; + case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); + pi=btVector3(mx.x(),mi.y(),mi.z());break; + case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); + pi=btVector3(mi.x(),mi.y(),mi.z());break; + } + if((dot(n,px)+o)<0) return(-1); + if((dot(n,pi)+o)>=0) return(+1); + return(0); +} + +// +DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const +{ + const btVector3* b[]={&mx,&mi}; + const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); + return(dot(p,v)); +} + +// +DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const +{ + for(int i=0;i<3;++i) + { + if(d[i]<0) + { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + else + { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + } +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); + const __int32* pu((const __int32*)&rt); + return((pu[0]|pu[1]|pu[2])==0); +#else + return( (a.mi.x()<=b.mx.x())&& + (a.mx.x()>=b.mi.x())&& + (a.mi.y()<=b.mx.y())&& + (a.mx.y()>=b.mi.y())&& + (a.mi.z()<=b.mx.z())&& + (a.mx.z()>=b.mi.z())); +#endif +} + + + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btVector3& b) +{ + return( (b.x()>=a.mi.x())&& + (b.y()>=a.mi.y())&& + (b.z()>=a.mi.z())&& + (b.x()<=a.mx.x())&& + (b.y()<=a.mx.y())&& + (b.z()<=a.mx.z())); +} + + + + + +////////////////////////////////////// + + +// +DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ + const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); +} + + + +// +DBVT_INLINE int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + ///@todo: the intrinsic version is 11% slower +#if DBVT_USE_INTRINSIC_SSE + + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + { + __m128 ssereg; + float floats[4]; + int ints[4]; + }; + + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + + btSSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + ATTRIBUTE_ALIGNED16(__int32 r[1]); + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); +#endif +#else + return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ + return( (a.mi.x()!=b.mi.x())|| + (a.mi.y()!=b.mi.y())|| + (a.mi.z()!=b.mi.z())|| + (a.mx.x()!=b.mx.x())|| + (a.mx.y()!=b.mx.y())|| + (a.mx.z()!=b.mx.z())); +} + +// +// Inline's +// + +// +DBVT_PREFIX +inline void btDbvt::enumNodes( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +DBVT_PREFIX +inline void btDbvt::enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + + + +DBVT_PREFIX +inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=m_stkStack[--depth]; + if(depth>treshold) + { + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +#if 0 +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + btAlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} +// +DBVT_PREFIX +inline void btDbvt::collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) +{ + const btTransform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); +} +#endif + +// +DBVT_PREFIX +inline void btDbvt::collideTV( const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + stack.resize(0); + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +DBVT_PREFIX +inline void btDbvt::rayTestInternal( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayDirectionInverse, + unsigned int signs[3], + btScalar lambda_max, + const btVector3& aabbMin, + const btVector3& aabbMax, + DBVT_IPOLICY) const +{ + DBVT_CHECKTYPE + if(root) + { + btVector3 resultNormal; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + btAlignedObjectArray stack; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + btVector3 bounds[2]; + do + { + const btDbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()+aabbMin; + bounds[1] = node->volume.Maxs()+aabbMax; + btScalar tmin=1.f,lambda_min=0.f; + unsigned int result1=false; + result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void btDbvt::rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + btVector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + + btVector3 resultNormal; + + btAlignedObjectArray stack; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + btVector3 bounds[2]; + do { + const btDbvtNode* node=stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + btScalar tmin=1.f,lambda_min=0.f; + unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + btScalar param=1.f; + bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(!out) + { + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) +{ + DBVT_CHECKTYPE + if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1< stock; + btAlignedObjectArray ifree; + btAlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); + } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideTU( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + +// +// PP Cleanup +// + +#undef DBVT_USE_MEMMOVE +#undef DBVT_USE_TEMPLATE +#undef DBVT_VIRTUAL_DTOR +#undef DBVT_VIRTUAL +#undef DBVT_PREFIX +#undef DBVT_IPOLICY +#undef DBVT_CHECKTYPE +#undef DBVT_IMPL_GENERIC +#undef DBVT_IMPL_SSE +#undef DBVT_USE_INTRINSIC_SSE +#undef DBVT_SELECT_IMPL +#undef DBVT_MERGE_IMPL +#undef DBVT_INT0_IMPL + +#endif diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 2c1212c48..f231717af 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -1,727 +1,727 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btDbvtBroadphase implementation by Nathanael Presson - -#include "btDbvtBroadphase.h" - -// -// Profiling -// - -#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK -#include -#endif - -#if DBVT_BP_PROFILE -struct ProfileScope -{ - __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) - { - } - __forceinline ~ProfileScope() - { - (*m_value)+=m_clock->getTimeMicroseconds()-m_base; - } - btClock* m_clock; - unsigned long* m_value; - unsigned long m_base; -}; -#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) -#else -#define SPC(_value_) -#endif - -// -// Helpers -// - -// -template -static inline void listappend(T* item,T*& list) -{ - item->links[0]=0; - item->links[1]=list; - if(list) list->links[0]=item; - list=item; -} - -// -template -static inline void listremove(T* item,T*& list) -{ - if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; - if(item->links[1]) item->links[1]->links[0]=item->links[0]; -} - -// -template -static inline int listcount(T* root) -{ - int n=0; - while(root) { ++n;root=root->links[1]; } - return(n); -} - -// -template -static inline void clear(T& value) -{ - static const struct ZeroDummy : T {} zerodummy; - value=zerodummy; -} - -// -// Colliders -// - -/* Tree collider */ -struct btDbvtTreeCollider : btDbvt::ICollide -{ - btDbvtBroadphase* pbp; - btDbvtProxy* proxy; - btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} - void Process(const btDbvtNode* na,const btDbvtNode* nb) - { - if(na!=nb) - { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; -#if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); -#endif - pbp->m_paircache->addOverlappingPair(pa,pb); - ++pbp->m_newpairs; - } - } - void Process(const btDbvtNode* n) - { - Process(n,proxy->leaf); - } -}; - -// -// btDbvtBroadphase -// - -// -btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) -{ - m_deferedcollide = false; - m_needcleanup = true; - m_releasepaircache = (paircache!=0)?false:true; - m_prediction = 1/(btScalar)2; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) - { - m_stageRoots[i]=0; - } -#if DBVT_BP_PROFILE - clear(m_profiling); -#endif -} - -// -btDbvtBroadphase::~btDbvtBroadphase() -{ - if(m_releasepaircache) - { - m_paircache->~btOverlappingPairCache(); - btAlignedFree(m_paircache); - } -} - -// -btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) -{ - btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, - collisionFilterGroup, - collisionFilterMask); - - btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - - //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); - proxy->stage = m_stageCurrent; - proxy->m_uniqueId = ++m_gid; - proxy->leaf = m_sets[0].insert(aabb,proxy); - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(!m_deferedcollide) - { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); - m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); - } - return(proxy); -} - -// -void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); - else - m_sets[0].remove(proxy->leaf); - listremove(proxy,m_stageRoots[proxy->stage]); - m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); - btAlignedFree(proxy); - m_needcleanup=true; -} - -void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - aabbMin = proxy->m_aabbMin; - aabbMax = proxy->m_aabbMax; -} - -struct BroadphaseRayTester : btDbvt::ICollide -{ - btBroadphaseRayCallback& m_rayCallback; - BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) - :m_rayCallback(orgCallback) - { - } - void Process(const btDbvtNode* leaf) - { - btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; - m_rayCallback.process(proxy); - } -}; - -void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) -{ - BroadphaseRayTester callback(rayCallback); - - m_sets[0].rayTestInternal( m_sets[0].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - - m_sets[1].rayTestInternal( m_sets[1].m_root, - rayFrom, - rayTo, - rayCallback.m_rayDirectionInverse, - rayCallback.m_signs, - rayCallback.m_lambda_max, - aabbMin, - aabbMax, - callback); - -} - -// -void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) -{ - btDbvtProxy* proxy=(btDbvtProxy*)absproxy; - ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); -#if DBVT_BP_PREVENTFALSEUPDATE - if(NotEqual(aabb,proxy->leaf->volume)) -#endif - { - bool docollide=false; - if(proxy->stage==STAGECOUNT) - {/* fixed -> dynamic set */ - m_sets[1].remove(proxy->leaf); - proxy->leaf=m_sets[0].insert(aabb,proxy); - docollide=true; - } - else - {/* dynamic set */ - ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) - {/* Moving */ - - const btVector3 delta=aabbMin-proxy->m_aabbMin; - btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( -#ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) -#else - m_sets[0].update(proxy->leaf,aabb,velocity) -#endif - ) - { - ++m_updates_done; - docollide=true; - } - } - else - {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); - ++m_updates_done; - docollide=true; - } - } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->m_aabbMin = aabbMin; - proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) - { - m_needcleanup=true; - if(!m_deferedcollide) - { - btDbvtTreeCollider collider(this); - m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); - } - } - } -} - -// -void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - collide(dispatcher); -#if DBVT_BP_PROFILE - if(0==(m_pid%DBVT_BP_PROFILING_RATE)) - { - printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); - unsigned int total=m_profiling.m_total; - if(total<=0) total=1; - printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); - printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); - printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); - printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); - const unsigned long sum=m_profiling.m_ddcollide+ - m_profiling.m_fdcollide+ - m_profiling.m_cleanup; - printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); - printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); - clear(m_profiling); - m_clock.reset(); - } -#endif - - performDeferredRemoval(dispatcher); - -} - -void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) -{ - - if (m_paircache->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - - int invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;ileaf->volume,pb->leaf->volume); - - if (hasOverlap) - { - needsRemoval = false; - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - m_paircache->cleanOverlappingPair(pair,dispatcher); - - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - invalidPair++; - } - - } - - //perform a sort, to sort 'invalid' pairs to the end - overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); - overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); - } -} - -// -void btDbvtBroadphase::collide(btDispatcher* dispatcher) -{ - /*printf("---------------------------------------------------------\n"); - printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); - printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); - printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); - { - int i; - for (i=0;igetNumOverlappingPairs();i++) - { - printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), - getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); - } - printf("\n"); - } -*/ - - - - SPC(m_profiling.m_total); - /* optimize */ - m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); - if(m_fixedleft) - { - const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; - m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); - m_fixedleft=btMax(0,m_fixedleft-count); - } - /* dynamic -> fixed set */ - m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; - btDbvtProxy* current=m_stageRoots[m_stageCurrent]; - if(current) - { - btDbvtTreeCollider collider(this); - do { - btDbvtProxy* next=current->links[1]; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); -#if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); -#endif - m_sets[0].remove(current->leaf); - ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); - current->leaf = m_sets[1].insert(curAabb,current); - current->stage = STAGECOUNT; - current = next; - } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; - } - /* collide dynamics */ - { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) - { - SPC(m_profiling.m_fdcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); - } - if(m_deferedcollide) - { - SPC(m_profiling.m_ddcollide); - m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); - } - } - /* clean up */ - if(m_needcleanup) - { - SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) - { - - int ni=btMin(pairs.size(),btMax(m_newpairs,(pairs.size()*m_cupdates)/100)); - for(int i=0;ileaf->volume,pb->leaf->volume)) - { -#if DBVT_BP_SORTPAIRS - if(pa->m_uniqueId>pb->m_uniqueId) - btSwap(pa,pb); -#endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; - } - } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; - } - } - ++m_pid; - m_newpairs=1; - m_needcleanup=false; - if(m_updates_call>0) - { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } - else - { m_updates_ratio=0; } - m_updates_done/=2; - m_updates_call/=2; -} - -// -void btDbvtBroadphase::optimize() -{ - m_sets[0].optimizeTopDown(); - m_sets[1].optimizeTopDown(); -} - -// -btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() -{ - return(m_paircache); -} - -// -const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const -{ - return(m_paircache); -} - -// -void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const -{ - - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; - - if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); - else - bounds=m_sets[0].m_root->volume; - else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; - else - bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); - aabbMin=bounds.Mins(); - aabbMax=bounds.Maxs(); -} - -void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) -{ - - int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; - if (!totalObjects) - { - //reset internal dynamic tree data structures - m_sets[0].clear(); - m_sets[1].clear(); - - m_deferedcollide = false; - m_needcleanup = true; - m_prediction = 1/(btScalar)2; - m_stageCurrent = 0; - m_fixedleft = 0; - m_fupdates = 1; - m_dupdates = 0; - m_cupdates = 10; - m_newpairs = 1; - m_updates_call = 0; - m_updates_done = 0; - m_updates_ratio = 0; - - m_gid = 0; - m_pid = 0; - m_cid = 0; - for(int i=0;i<=STAGECOUNT;++i) - { - m_stageRoots[i]=0; - } - } -} - -// -void btDbvtBroadphase::printStats() -{} - -// -#if DBVT_BP_ENABLE_BENCHMARK - -struct btBroadphaseBenchmark -{ - struct Experiment - { - const char* name; - int object_count; - int update_count; - int spawn_count; - int iterations; - btScalar speed; - btScalar amplitude; - }; - struct Object - { - btVector3 center; - btVector3 extents; - btBroadphaseProxy* proxy; - btScalar time; - void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) - { - time += speed; - center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; - center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; - center[2] = btSin(time*(btScalar)0.777)*amplitude; - pbi->setAabb(proxy,center-extents,center+extents,0); - } - }; - static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } - static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } - static void OutputTime(const char* name,btClock& c,unsigned count=0) - { - const unsigned long us=c.getTimeMicroseconds(); - const unsigned long ms=(us+500)/1000; - const btScalar sec=us/(btScalar)(1000*1000); - if(count>0) - printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); - else - printf("%s : %u us (%u ms)\r\n",name,us,ms); - } -}; - -void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) -{ - static const btBroadphaseBenchmark::Experiment experiments[]= - { - {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, - /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, - {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ - }; - static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); - btAlignedObjectArray objects; - btClock wallclock; - /* Begin */ - for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); - objects.push_back(po); - } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); - } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); - } - pbi->calculateOverlappingPairs(0); - } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ - wallclock.reset(); - for(int i=0;idestroyProxy(objects[i]->proxy,0); - delete objects[i]; - } - objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); - } - -} -#else -void btDbvtBroadphase::benchmark(btBroadphaseInterface*) -{} -#endif - -#if DBVT_BP_PROFILE -#undef SPC -#endif - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvtBroadphase implementation by Nathanael Presson + +#include "btDbvtBroadphase.h" + +// +// Profiling +// + +#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#include +#endif + +#if DBVT_BP_PROFILE +struct ProfileScope +{ + __forceinline ProfileScope(btClock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; +}; +#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#else +#define SPC(_value_) +#endif + +// +// Helpers +// + +// +template +static inline void listappend(T* item,T*& list) +{ + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; +} + +// +template +static inline void listremove(T* item,T*& list) +{ + if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; + if(item->links[1]) item->links[1]->links[0]=item->links[0]; +} + +// +template +static inline int listcount(T* root) +{ + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); +} + +// +template +static inline void clear(T& value) +{ + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct btDbvtTreeCollider : btDbvt::ICollide +{ + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} + void Process(const btDbvtNode* na,const btDbvtNode* nb) + { + if(na!=nb) + { + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa,pb); + ++pbp->m_newpairs; + } + } + void Process(const btDbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// btDbvtBroadphase +// + +// +btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) +{ + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if DBVT_BP_PROFILE + clear(m_profiling); +#endif +} + +// +btDbvtBroadphase::~btDbvtBroadphase() +{ + if(m_releasepaircache) + { + m_paircache->~btOverlappingPairCache(); + btAlignedFree(m_paircache); + } +} + +// +btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) +{ + btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + + //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = ++m_gid; + proxy->leaf = m_sets[0].insert(aabb,proxy); + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); + m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); + } + return(proxy); +} + +// +void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); + listremove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); + btAlignedFree(proxy); + m_needcleanup=true; +} + +void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} + +struct BroadphaseRayTester : btDbvt::ICollide +{ + btBroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_rayCallback.process(proxy); + } +}; + +void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) +{ + BroadphaseRayTester callback(rayCallback); + + m_sets[0].rayTestInternal( m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + + m_sets[1].rayTestInternal( m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + +} + +// +void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); +#if DBVT_BP_PREVENTFALSEUPDATE + if(NotEqual(aabb,proxy->leaf->volume)) +#endif + { + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + + const btVector3 delta=aabbMin-proxy->m_aabbMin; + btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( +#ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) +#else + m_sets[0].update(proxy->leaf,aabb,velocity) +#endif + ) + { + ++m_updates_done; + docollide=true; + } + } + else + {/* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } + } +} + +// +void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + collide(dispatcher); +#if DBVT_BP_PROFILE + if(0==(m_pid%DBVT_BP_PROFILING_RATE)) + { + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); + } +#endif + + performDeferredRemoval(dispatcher); + +} + +void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher) +{ + + if (m_paircache->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + int invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;ileaf->volume,pb->leaf->volume); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } +} + +// +void btDbvtBroadphase::collide(btDispatcher* dispatcher) +{ + /*printf("---------------------------------------------------------\n"); + printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); + printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); + printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); + { + int i; + for (i=0;igetNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); + } +*/ + + + + SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax(0,m_fixedleft-count); + } + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + btDbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) + { + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); +#if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb,current); + current->stage = STAGECOUNT; + current = next; + } while(current); + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; + } + /* collide dynamics */ + { + btDbvtTreeCollider collider(this); + if(m_deferedcollide) + { + SPC(m_profiling.m_fdcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + SPC(m_profiling.m_ddcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } + /* clean up */ + if(m_needcleanup) + { + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + + int ni=btMin(pairs.size(),btMax(m_newpairs,(pairs.size()*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + { +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; + } + } + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + } + } + ++m_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) + { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } + else + { m_updates_ratio=0; } + m_updates_done/=2; + m_updates_call/=2; +} + +// +void btDbvtBroadphase::optimize() +{ + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); +} + +// +btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() +{ + return(m_paircache); +} + +// +const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const +{ + return(m_paircache); +} + +// +void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const +{ + + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; + + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; + else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); +} + +void btDbvtBroadphase::resetPool(btDispatcher* dispatcher) +{ + + int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; + if (!totalObjects) + { + //reset internal dynamic tree data structures + m_sets[0].clear(); + m_sets[1].clear(); + + m_deferedcollide = false; + m_needcleanup = true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } +} + +// +void btDbvtBroadphase::printStats() +{} + +// +#if DBVT_BP_ENABLE_BENCHMARK + +struct btBroadphaseBenchmark +{ + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + btScalar speed; + btScalar amplitude; + }; + struct Object + { + btVector3 center; + btVector3 extents; + btBroadphaseProxy* proxy; + btScalar time; + void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) + { + time += speed; + center[0] = btCos(time*(btScalar)2.17)*amplitude+ + btSin(time)*amplitude/2; + center[1] = btCos(time*(btScalar)1.38)*amplitude+ + btSin(time)*amplitude; + center[2] = btSin(time*(btScalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } + static void OutputTime(const char* name,btClock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const btScalar sec=us/(btScalar)(1000*1000); + if(count>0) + printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); + else + printf("%s : %u us (%u ms)\r\n",name,us,ms); + } +}; + +void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) +{ + static const btBroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ + }; + static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); + btAlignedObjectArray objects; + btClock wallclock; + /* Begin */ + for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;idestroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +void btDbvtBroadphase::benchmark(btBroadphaseInterface*) +{} +#endif + +#if DBVT_BP_PROFILE +#undef SPC +#endif + diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 13b3c6207..fe70bc39c 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -1,126 +1,126 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btDbvtBroadphase implementation by Nathanael Presson -#ifndef BT_DBVT_BROADPHASE_H -#define BT_DBVT_BROADPHASE_H - -#include "BulletCollision/BroadphaseCollision/btDbvt.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" - -// -// Compile time config -// - -#define DBVT_BP_PROFILE 0 -//#define DBVT_BP_SORTPAIRS 1 -#define DBVT_BP_PREVENTFALSEUPDATE 0 -#define DBVT_BP_ACCURATESLEEPING 0 -#define DBVT_BP_ENABLE_BENCHMARK 0 -#define DBVT_BP_MARGIN (btScalar)0.05 - -#if DBVT_BP_PROFILE -#define DBVT_BP_PROFILING_RATE 256 -#include "LinearMath/btQuickprof.h" -#endif - -// -// btDbvtProxy -// -struct btDbvtProxy : btBroadphaseProxy -{ - /* Fields */ - //btDbvtAabbMm aabb; - btDbvtNode* leaf; - btDbvtProxy* links[2]; - int stage; - /* ctor */ - btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : - btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) - { - links[0]=links[1]=0; - } -}; - -typedef btAlignedObjectArray btDbvtProxyArray; - -///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). -///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. -///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. -struct btDbvtBroadphase : btBroadphaseInterface -{ - /* Config */ - enum { - DYNAMIC_SET = 0, /* Dynamic set index */ - FIXED_SET = 1, /* Fixed set index */ - STAGECOUNT = 2 /* Number of stages */ - }; - /* Fields */ - btDbvt m_sets[2]; // Dbvt sets - btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list - btOverlappingPairCache* m_paircache; // Pair cache - btScalar m_prediction; // Velocity prediction - int m_stageCurrent; // Current stage - int m_fupdates; // % of fixed updates per frame - int m_dupdates; // % of dynamic updates per frame - int m_cupdates; // % of cleanup updates per frame - int m_newpairs; // Number of pairs created - int m_fixedleft; // Fixed optimization left - unsigned m_updates_call; // Number of updates call - unsigned m_updates_done; // Number of updates done - btScalar m_updates_ratio; // m_updates_done/m_updates_call - int m_pid; // Parse id - int m_cid; // Cleanup index - int m_gid; // Gen id - bool m_releasepaircache; // Release pair cache on delete - bool m_deferedcollide; // Defere dynamic/static collision to collide call - bool m_needcleanup; // Need to run cleanup? -#if DBVT_BP_PROFILE - btClock m_clock; - struct { - unsigned long m_total; - unsigned long m_ddcollide; - unsigned long m_fdcollide; - unsigned long m_cleanup; - unsigned long m_jobcount; - } m_profiling; -#endif - /* Methods */ - btDbvtBroadphase(btOverlappingPairCache* paircache=0); - ~btDbvtBroadphase(); - void collide(btDispatcher* dispatcher); - void optimize(); - /* btBroadphaseInterface Implementation */ - btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); - void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); - - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - void calculateOverlappingPairs(btDispatcher* dispatcher); - btOverlappingPairCache* getOverlappingPairCache(); - const btOverlappingPairCache* getOverlappingPairCache() const; - void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; - void printStats(); - static void benchmark(btBroadphaseInterface*); - - - void performDeferredRemoval(btDispatcher* dispatcher); - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - -}; - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btDbvtBroadphase implementation by Nathanael Presson +#ifndef BT_DBVT_BROADPHASE_H +#define BT_DBVT_BROADPHASE_H + +#include "BulletCollision/BroadphaseCollision/btDbvt.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" + +// +// Compile time config +// + +#define DBVT_BP_PROFILE 0 +//#define DBVT_BP_SORTPAIRS 1 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +#define DBVT_BP_MARGIN (btScalar)0.05 + +#if DBVT_BP_PROFILE +#define DBVT_BP_PROFILING_RATE 256 +#include "LinearMath/btQuickprof.h" +#endif + +// +// btDbvtProxy +// +struct btDbvtProxy : btBroadphaseProxy +{ + /* Fields */ + //btDbvtAabbMm aabb; + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef btAlignedObjectArray btDbvtProxyArray; + +///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). +///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. +///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. +struct btDbvtBroadphase : btBroadphaseInterface +{ + /* Config */ + enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; + /* Fields */ + btDbvt m_sets[2]; // Dbvt sets + btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + btOverlappingPairCache* m_paircache; // Pair cache + btScalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + btScalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + int m_gid; // Gen id + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? +#if DBVT_BP_PROFILE + btClock m_clock; + struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; +#endif + /* Methods */ + btDbvtBroadphase(btOverlappingPairCache* paircache=0); + ~btDbvtBroadphase(); + void collide(btDispatcher* dispatcher); + void optimize(); + /* btBroadphaseInterface Implementation */ + btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); + + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + void calculateOverlappingPairs(btDispatcher* dispatcher); + btOverlappingPairCache* getOverlappingPairCache(); + const btOverlappingPairCache* getOverlappingPairCache() const; + void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; + void printStats(); + static void benchmark(btBroadphaseInterface*); + + + void performDeferredRemoval(btDispatcher* dispatcher); + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + +}; + +#endif diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index ef6779240..6712f528e 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -1,489 +1,489 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btMultiSapBroadphase.h" - -#include "btSimpleBroadphase.h" -#include "LinearMath/btAabbUtil2.h" -#include "btQuantizedBvh.h" - -/// btSapBroadphaseArray m_sapBroadphases; - -/// btOverlappingPairCache* m_overlappingPairs; -extern int gOverlappingPairs; - -/* -class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache -{ -public: - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); - } -}; - -*/ - -btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) -:m_overlappingPairs(pairCache), -m_optimizedAabbTree(0), -m_ownsPairCache(false), -m_invalidPair(0) -{ - if (!m_overlappingPairs) - { - m_ownsPairCache = true; - void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); - m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); - } - - struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback - { - virtual ~btMultiSapOverlapFilterCallback() - {} - // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const - { - btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; - btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; - - bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; - collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); - - return collides; - } - }; - - void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); - m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); - - m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); -// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); -// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); -} - -btMultiSapBroadphase::~btMultiSapBroadphase() -{ - if (m_ownsPairCache) - { - m_overlappingPairs->~btOverlappingPairCache(); - btAlignedFree(m_overlappingPairs); - } -} - - -void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) -{ - m_optimizedAabbTree = new btQuantizedBvh(); - m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); - QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); - for (int i=0;igetBroadphaseAabb(aabbMin,aabbMax); - m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); - m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); - int partId = 0; - node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; - nodes.push_back(node); - } - m_optimizedAabbTree->buildInternal(); -} - -btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) -{ - //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested - - void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); - btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); - m_multiSapProxies.push_back(proxy); - - ///this should deal with inserting/removal into child broadphases - setAabb(proxy,aabbMin,aabbMax,dispatcher); - return proxy; -} - -void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) -{ - ///not yet - btAssert(0); - -} - - -void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) -{ - void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); - btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; - bridgeProxyRef->m_childProxy = childProxy; - bridgeProxyRef->m_childBroadphase = childBroadphase; - parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); -} - - -bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); -bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) -{ -return -amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && -amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && -amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); -} - - - - - - -void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const -{ - btMultiSapProxy* multiProxy = static_cast(proxy); - aabbMin = multiProxy->m_aabbMin; - aabbMax = multiProxy->m_aabbMax; -} - -void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) -{ - for (int i=0;i - -void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) -{ - btMultiSapProxy* multiProxy = static_cast(proxy); - multiProxy->m_aabbMin = aabbMin; - multiProxy->m_aabbMax = aabbMax; - - -// bool fullyContained = false; -// bool alreadyInSimple = false; - - - - - struct MyNodeOverlapCallback : public btNodeOverlapCallback - { - btMultiSapBroadphase* m_multiSap; - btMultiSapProxy* m_multiProxy; - btDispatcher* m_dispatcher; - - MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) - :m_multiSap(multiSap), - m_multiProxy(multiProxy), - m_dispatcher(dispatcher) - { - - } - - virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) - { - btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; - - int containingBroadphaseIndex = -1; - //already found? - for (int i=0;im_bridgeProxies.size();i++) - { - - if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - break; - } - } - if (containingBroadphaseIndex<0) - { - //add it - btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); - m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); - - } - } - }; - - MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); - - - - - if (m_optimizedAabbTree) - m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); - - int i; - - for ( i=0;im_bridgeProxies.size();i++) - { - btVector3 worldAabbMin,worldAabbMax; - multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - if (!overlapsBroadphase) - { - //remove it now - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - - } - } - - - /* - - if (1) - { - - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;igetBroadphaseAabb(worldAabbMin,worldAabbMax); - bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - - // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); - int containingBroadphaseIndex = -1; - - //if already contains this - - for (int i=0;im_bridgeProxies.size();i++) - { - if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) - { - containingBroadphaseIndex = i; - } - alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); - } - - if (overlapsBroadphase) - { - if (containingBroadphaseIndex<0) - { - btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,childBroadphase); - } - } else - { - if (containingBroadphaseIndex>=0) - { - //remove - btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; - - btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; - bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); - - multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); - multiProxy->m_bridgeProxies.pop_back(); - } - } - } - - - ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) - ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) - if (0)//!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } - } - - if (!multiProxy->m_bridgeProxies.size()) - { - ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision - ///this is needed to be able to calculate the aabb overlap - btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); - childProxy->m_multiSapParentProxy = multiProxy; - addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); - } -*/ - - - //update - for ( i=0;im_bridgeProxies.size();i++) - { - btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; - bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); - } - -} -bool stopUpdating=false; - - - -class btMultiSapBroadphasePairSortPredicate -{ - public: - - bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) - { - btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; - - return aProxy0 > bProxy0 || - (aProxy0 == bProxy0 && aProxy1 > bProxy1) || - (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); - } -}; - - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb -void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) -{ - -// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); - - if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) - { - - btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); - - // quicksort(overlappingPairArray,0,overlappingPairArray.size()); - - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - //perform a sort, to find duplicates and to sort 'invalid' pairs to the end - // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - - - int i; - - btBroadphasePair previousPair; - previousPair.m_pProxy0 = 0; - previousPair.m_pProxy1 = 0; - previousPair.m_algorithm = 0; - - - for (i=0;im_multiSapParentProxy : 0; - btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; - btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; - - bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); - - previousPair = pair; - - bool needsRemoval = false; - - if (!isDuplicate) - { - bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); - - if (hasOverlap) - { - needsRemoval = false;//callback->processOverlap(pair); - } else - { - needsRemoval = true; - } - } else - { - //remove duplicate - needsRemoval = true; - //should have no algorithm - btAssert(!pair.m_algorithm); - } - - if (needsRemoval) - { - getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); - - // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - // m_overlappingPairArray.pop_back(); - pair.m_pProxy0 = 0; - pair.m_pProxy1 = 0; - m_invalidPair++; - gOverlappingPairs--; - } - - } - - ///if you don't like to skip the invalid pairs in the array, execute following code: - #define CLEAN_INVALID_PAIRS 1 - #ifdef CLEAN_INVALID_PAIRS - - //perform a sort, to sort 'invalid' pairs to the end - //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); - overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); - - overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); - m_invalidPair = 0; - #endif//CLEAN_INVALID_PAIRS - - //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); - } - - -} - - -bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) -{ - btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; - btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; - - return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, - multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); - -} - - -void btMultiSapBroadphase::printStats() -{ -/* printf("---------------------------------\n"); - - printf("btMultiSapBroadphase.h\n"); - printf("numHandles = %d\n",m_multiSapProxies.size()); - //find broadphase that contain this multiProxy - int numChildBroadphases = getBroadphaseArray().size(); - for (int i=0;iprintStats(); - - } - */ - -} - -void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) -{ - // not yet -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btMultiSapBroadphase.h" + +#include "btSimpleBroadphase.h" +#include "LinearMath/btAabbUtil2.h" +#include "btQuantizedBvh.h" + +/// btSapBroadphaseArray m_sapBroadphases; + +/// btOverlappingPairCache* m_overlappingPairs; +extern int gOverlappingPairs; + +/* +class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache +{ +public: + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); + } +}; + +*/ + +btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) +:m_overlappingPairs(pairCache), +m_optimizedAabbTree(0), +m_ownsPairCache(false), +m_invalidPair(0) +{ + if (!m_overlappingPairs) + { + m_ownsPairCache = true; + void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); + m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); + } + + struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback + { + virtual ~btMultiSapOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const + { + btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; + btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; + + bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; + collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); + + return collides; + } + }; + + void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); + m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); + + m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); +// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); +// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); +} + +btMultiSapBroadphase::~btMultiSapBroadphase() +{ + if (m_ownsPairCache) + { + m_overlappingPairs->~btOverlappingPairCache(); + btAlignedFree(m_overlappingPairs); + } +} + + +void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) +{ + m_optimizedAabbTree = new btQuantizedBvh(); + m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); + QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); + for (int i=0;igetBroadphaseAabb(aabbMin,aabbMax); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); + m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); + int partId = 0; + node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; + nodes.push_back(node); + } + m_optimizedAabbTree->buildInternal(); +} + +btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* /*ignoreMe*/) +{ + //void* ignoreMe -> we could think of recursive multi-sap, if someone is interested + + void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); + btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); + m_multiSapProxies.push_back(proxy); + + ///this should deal with inserting/removal into child broadphases + setAabb(proxy,aabbMin,aabbMax,dispatcher); + return proxy; +} + +void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) +{ + ///not yet + btAssert(0); + +} + + +void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) +{ + void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); + btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; + bridgeProxyRef->m_childProxy = childProxy; + bridgeProxyRef->m_childBroadphase = childBroadphase; + parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); +} + + +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax); +bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax) +{ +return +amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && +amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && +amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); +} + + + + + + +void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const +{ + btMultiSapProxy* multiProxy = static_cast(proxy); + aabbMin = multiProxy->m_aabbMin; + aabbMax = multiProxy->m_aabbMax; +} + +void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) +{ + for (int i=0;i + +void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) +{ + btMultiSapProxy* multiProxy = static_cast(proxy); + multiProxy->m_aabbMin = aabbMin; + multiProxy->m_aabbMax = aabbMax; + + +// bool fullyContained = false; +// bool alreadyInSimple = false; + + + + + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btMultiSapBroadphase* m_multiSap; + btMultiSapProxy* m_multiProxy; + btDispatcher* m_dispatcher; + + MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) + :m_multiSap(multiSap), + m_multiProxy(multiProxy), + m_dispatcher(dispatcher) + { + + } + + virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) + { + btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; + + int containingBroadphaseIndex = -1; + //already found? + for (int i=0;im_bridgeProxies.size();i++) + { + + if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + break; + } + } + if (containingBroadphaseIndex<0) + { + //add it + btBroadphaseProxy* childProxy = childBroadphase->createProxy(m_multiProxy->m_aabbMin,m_multiProxy->m_aabbMax,m_multiProxy->m_shapeType,m_multiProxy->m_clientObject,m_multiProxy->m_collisionFilterGroup,m_multiProxy->m_collisionFilterMask, m_dispatcher,m_multiProxy); + m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase); + + } + } + }; + + MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); + + + + + if (m_optimizedAabbTree) + m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + + int i; + + for ( i=0;im_bridgeProxies.size();i++) + { + btVector3 worldAabbMin,worldAabbMax; + multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + if (!overlapsBroadphase) + { + //remove it now + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + + } + } + + + /* + + if (1) + { + + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;igetBroadphaseAabb(worldAabbMin,worldAabbMax); + bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + + // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); + int containingBroadphaseIndex = -1; + + //if already contains this + + for (int i=0;im_bridgeProxies.size();i++) + { + if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) + { + containingBroadphaseIndex = i; + } + alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); + } + + if (overlapsBroadphase) + { + if (containingBroadphaseIndex<0) + { + btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,childBroadphase); + } + } else + { + if (containingBroadphaseIndex>=0) + { + //remove + btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; + + btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; + bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); + + multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); + multiProxy->m_bridgeProxies.pop_back(); + } + } + } + + + ///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) + ///hopefully we don't end up with many entries here (can assert/provide feedback on stats) + if (0)//!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } + } + + if (!multiProxy->m_bridgeProxies.size()) + { + ///we don't pass the userPtr but our multisap proxy. We need to patch this, before processing an actual collision + ///this is needed to be able to calculate the aabb overlap + btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); + childProxy->m_multiSapParentProxy = multiProxy; + addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); + } +*/ + + + //update + for ( i=0;im_bridgeProxies.size();i++) + { + btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; + bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); + } + +} +bool stopUpdating=false; + + + +class btMultiSapBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) + { + btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy0 = b1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0; + + return aProxy0 > bProxy0 || + (aProxy0 == bProxy0 && aProxy1 > bProxy1) || + (aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); + } +}; + + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb +void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + +// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); + + if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); + + // quicksort(overlappingPairArray,0,overlappingPairArray.size()); + + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + + + int i; + + btBroadphasePair previousPair; + previousPair.m_pProxy0 = 0; + previousPair.m_pProxy1 = 0; + previousPair.m_algorithm = 0; + + + for (i=0;im_multiSapParentProxy : 0; + btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0; + btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; + + bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); + + previousPair = pair; + + bool needsRemoval = false; + + if (!isDuplicate) + { + bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); + + if (hasOverlap) + { + needsRemoval = false;//callback->processOverlap(pair); + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + btAssert(!pair.m_algorithm); + } + + if (needsRemoval) + { + getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); + + // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + // m_overlappingPairArray.pop_back(); + pair.m_pProxy0 = 0; + pair.m_pProxy1 = 0; + m_invalidPair++; + gOverlappingPairs--; + } + + } + + ///if you don't like to skip the invalid pairs in the array, execute following code: + #define CLEAN_INVALID_PAIRS 1 + #ifdef CLEAN_INVALID_PAIRS + + //perform a sort, to sort 'invalid' pairs to the end + //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); + overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); + + overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); + m_invalidPair = 0; + #endif//CLEAN_INVALID_PAIRS + + //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); + } + + +} + + +bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) +{ + btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; + btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; + + return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, + multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); + +} + + +void btMultiSapBroadphase::printStats() +{ +/* printf("---------------------------------\n"); + + printf("btMultiSapBroadphase.h\n"); + printf("numHandles = %d\n",m_multiSapProxies.size()); + //find broadphase that contain this multiProxy + int numChildBroadphases = getBroadphaseArray().size(); + for (int i=0;iprintStats(); + + } + */ + +} + +void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) +{ + // not yet +} diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h index 1e7bb28a4..91c504eee 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -1,151 +1,151 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BT_MULTI_SAP_BROADPHASE -#define BT_MULTI_SAP_BROADPHASE - -#include "btBroadphaseInterface.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "btOverlappingPairCache.h" - - -class btBroadphaseInterface; -class btSimpleBroadphase; - - -typedef btAlignedObjectArray btSapBroadphaseArray; - -///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. -///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. -///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. -///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. -///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 -///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 -class btMultiSapBroadphase :public btBroadphaseInterface -{ - btSapBroadphaseArray m_sapBroadphases; - - btSimpleBroadphase* m_simpleBroadphase; - - btOverlappingPairCache* m_overlappingPairs; - - class btQuantizedBvh* m_optimizedAabbTree; - - - bool m_ownsPairCache; - - btOverlapFilterCallback* m_filterCallback; - - int m_invalidPair; - - struct btBridgeProxy - { - btBroadphaseProxy* m_childProxy; - btBroadphaseInterface* m_childBroadphase; - }; - - -public: - - struct btMultiSapProxy : public btBroadphaseProxy - { - - ///array with all the entries that this proxy belongs to - btAlignedObjectArray m_bridgeProxies; - btVector3 m_aabbMin; - btVector3 m_aabbMax; - - int m_shapeType; - -/* void* m_userPtr; - short int m_collisionFilterGroup; - short int m_collisionFilterMask; -*/ - btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) - :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), - m_aabbMin(aabbMin), - m_aabbMax(aabbMax), - m_shapeType(shapeType) - { - m_multiSapParentProxy =this; - } - - - }; - -protected: - - - btAlignedObjectArray m_multiSapProxies; - -public: - - btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); - - - btSapBroadphaseArray& getBroadphaseArray() - { - return m_sapBroadphases; - } - - const btSapBroadphaseArray& getBroadphaseArray() const - { - return m_sapBroadphases; - } - - virtual ~btMultiSapBroadphase(); - - virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); - virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); - virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; - - virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); - - void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); - - ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - - bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - virtual btOverlappingPairCache* getOverlappingPairCache() - { - return m_overlappingPairs; - } - virtual const btOverlappingPairCache* getOverlappingPairCache() const - { - return m_overlappingPairs; - } - - ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame - ///will add some transform later - virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin.setValue(-1e30f,-1e30f,-1e30f); - aabbMax.setValue(1e30f,1e30f,1e30f); - } - - void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); - - virtual void printStats(); - - void quicksort (btBroadphasePairArray& a, int lo, int hi); - - ///reset broadphase internal structures, to ensure determinism/reproducability - virtual void resetPool(btDispatcher* dispatcher); - -}; - -#endif //BT_MULTI_SAP_BROADPHASE +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BT_MULTI_SAP_BROADPHASE +#define BT_MULTI_SAP_BROADPHASE + +#include "btBroadphaseInterface.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "btOverlappingPairCache.h" + + +class btBroadphaseInterface; +class btSimpleBroadphase; + + +typedef btAlignedObjectArray btSapBroadphaseArray; + +///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. +///The btMultiSapBroadphase is a broadphase that contains multiple SAP broadphases. +///The user can add SAP broadphases that cover the world. A btBroadphaseProxy can be in multiple child broadphases at the same time. +///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy. +///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 +///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 +class btMultiSapBroadphase :public btBroadphaseInterface +{ + btSapBroadphaseArray m_sapBroadphases; + + btSimpleBroadphase* m_simpleBroadphase; + + btOverlappingPairCache* m_overlappingPairs; + + class btQuantizedBvh* m_optimizedAabbTree; + + + bool m_ownsPairCache; + + btOverlapFilterCallback* m_filterCallback; + + int m_invalidPair; + + struct btBridgeProxy + { + btBroadphaseProxy* m_childProxy; + btBroadphaseInterface* m_childBroadphase; + }; + + +public: + + struct btMultiSapProxy : public btBroadphaseProxy + { + + ///array with all the entries that this proxy belongs to + btAlignedObjectArray m_bridgeProxies; + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + int m_shapeType; + +/* void* m_userPtr; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; +*/ + btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) + :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax), + m_shapeType(shapeType) + { + m_multiSapParentProxy =this; + } + + + }; + +protected: + + + btAlignedObjectArray m_multiSapProxies; + +public: + + btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); + + + btSapBroadphaseArray& getBroadphaseArray() + { + return m_sapBroadphases; + } + + const btSapBroadphaseArray& getBroadphaseArray() const + { + return m_sapBroadphases; + } + + virtual ~btMultiSapBroadphase(); + + virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin=btVector3(0,0,0),const btVector3& aabbMax=btVector3(0,0,0)); + + void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase); + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); + + bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + virtual btOverlappingPairCache* getOverlappingPairCache() + { + return m_overlappingPairs; + } + virtual const btOverlappingPairCache* getOverlappingPairCache() const + { + return m_overlappingPairs; + } + + ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame + ///will add some transform later + virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + } + + void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); + + virtual void printStats(); + + void quicksort (btBroadphasePairArray& a, int lo, int hi); + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + +}; + +#endif //BT_MULTI_SAP_BROADPHASE diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 9cf98dbae..b209bcb9a 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -1,633 +1,633 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btOverlappingPairCache.h" - -#include "btDispatcher.h" -#include "btCollisionAlgorithm.h" -#include "LinearMath/btAabbUtil2.h" - -#include - -int gOverlappingPairs = 0; - -int gRemovePairs =0; -int gAddedPairs =0; -int gFindPairs =0; - - - - -btHashedOverlappingPairCache::btHashedOverlappingPairCache(): - m_overlapFilterCallback(0), - m_blockedForChanges(false), - m_ghostPairCallback(0) -{ - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); - growTables(); -} - - - - -btHashedOverlappingPairCache::~btHashedOverlappingPairCache() -{ -} - - - -void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) -{ - if (pair.m_algorithm) - { - { - pair.m_algorithm->~btCollisionAlgorithm(); - dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; - } - } -} - - - - -void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class CleanPairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; - btDispatcher* m_dispatcher; - - public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - if ((pair.m_pProxy0 == m_cleanProxy) || - (pair.m_pProxy1 == m_cleanProxy)) - { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); - } - return false; - } - - }; - - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); - -} - - - - -void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class RemovePairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_obsoleteProxy; - - public: - RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); - } - - }; - - - RemovePairCallback removeCallback(proxy); - - processAllOverlappingPairs(&removeCallback,dispatcher); -} - - - - - -btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) -{ - gFindPairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); - - if (hash >= m_hashTable.size()) - { - return NULL; - } - - int index = m_hashTable[hash]; - while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) - { - index = m_next[index]; - } - - if (index == BT_NULL_PAIR) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; -} - -//#include - -void btHashedOverlappingPairCache::growTables() -{ - - int newCapacity = m_overlappingPairArray.capacity(); - - if (m_hashTable.size() < newCapacity) - { - //grow hashtable and next table - int curHashtableSize = m_hashTable.size(); - - m_hashTable.resize(newCapacity); - m_next.resize(newCapacity); - - - int i; - - for (i= 0; i < newCapacity; ++i) - { - m_hashTable[i] = BT_NULL_PAIR; - } - for (i = 0; i < newCapacity; ++i) - { - m_next[i] = BT_NULL_PAIR; - } - - for(i=0;igetUid(); - int proxyId2 = pair.m_pProxy1->getUid(); - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - int hashValue = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - m_next[i] = m_hashTable[hashValue]; - m_hashTable[hashValue] = i; - } - - - } -} - -btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) -{ - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask - - - btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); - if (pair != NULL) - { - return pair; - } - /*for(int i=0;i%u\r\n",proxyId1,proxyId2); - internalFindPair(proxy0, proxy1, hash); - } - }*/ - int count = m_overlappingPairArray.size(); - int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expand(); - - //this is where we add an actual pair, so also call the 'ghost' - if (m_ghostPairCallback) - m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); - - int newCapacity = m_overlappingPairArray.capacity(); - - if (oldCapacity < newCapacity) - { - growTables(); - //hash with new capacity - hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); - } - - pair = new (mem) btBroadphasePair(*proxy0,*proxy1); -// pair->m_pProxy0 = proxy0; -// pair->m_pProxy1 = proxy1; - pair->m_algorithm = 0; - pair->m_internalTmpValue = 0; - - - m_next[count] = m_hashTable[hash]; - m_hashTable[hash] = count; - - return pair; -} - - - -void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) -{ - gRemovePairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) - btSwap(proxy0,proxy1); - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - - /*if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2);*/ - - int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); - - btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); - if (pair == NULL) - { - return 0; - } - - cleanOverlappingPair(*pair,dispatcher); - - void* userData = pair->m_internalInfo1; - - btAssert(pair->m_pProxy0->getUid() == proxyId1); - btAssert(pair->m_pProxy1->getUid() == proxyId2); - - int pairIndex = int(pair - &m_overlappingPairArray[0]); - btAssert(pairIndex < m_overlappingPairArray.size()); - - // Remove the pair from the hash table. - int index = m_hashTable[hash]; - btAssert(index != BT_NULL_PAIR); - - int previous = BT_NULL_PAIR; - while (index != pairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_NULL_PAIR) - { - btAssert(m_next[previous] == pairIndex); - m_next[previous] = m_next[pairIndex]; - } - else - { - m_hashTable[hash] = m_next[pairIndex]; - } - - // We now move the last pair into spot of the - // pair being removed. We need to fix the hash - // table indices to support the move. - - int lastPairIndex = m_overlappingPairArray.size() - 1; - - if (m_ghostPairCallback) - m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); - - // If the removed pair is the last pair, we are done. - if (lastPairIndex == pairIndex) - { - m_overlappingPairArray.pop_back(); - return userData; - } - - // Remove the last pair from the hash table. - const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; - /* missing swap here too, Nat. */ - int lastHash = static_cast(getHash(static_cast(last->m_pProxy0->getUid()), static_cast(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1)); - - index = m_hashTable[lastHash]; - btAssert(index != BT_NULL_PAIR); - - previous = BT_NULL_PAIR; - while (index != lastPairIndex) - { - previous = index; - index = m_next[index]; - } - - if (previous != BT_NULL_PAIR) - { - btAssert(m_next[previous] == lastPairIndex); - m_next[previous] = m_next[lastPairIndex]; - } - else - { - m_hashTable[lastHash] = m_next[lastPairIndex]; - } - - // Copy the last pair into the remove pair's spot. - m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; - - // Insert the last pair into the hash table - m_next[pairIndex] = m_hashTable[lastHash]; - m_hashTable[lastHash] = pairIndex; - - m_overlappingPairArray.pop_back(); - - return userData; -} -//#include - -void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) -{ - - int i; - -// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); - for (i=0;iprocessOverlap(*pair)) - { - removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); - - gOverlappingPairs--; - } else - { - i++; - } - } -} - -void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) -{ - ///need to keep hashmap in sync with pair address, so rebuild all - btBroadphasePairArray tmpPairs; - int i; - for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); - - m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); - m_overlappingPairArray.pop_back(); - return userData; - } - } - - return 0; -} - - - - - - - - -btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - //don't add overlap with own - btAssert(proxy0 != proxy1); - - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - void* mem = &m_overlappingPairArray.expand(); - btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); - - gOverlappingPairs++; - gAddedPairs++; - - if (m_ghostPairCallback) - m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); - return pair; - -} - -///this findPair becomes really slow. Either sort the list to speedup the query, or -///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. -///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) -///Also we can use a 2D bitmap, which can be useful for a future GPU implementation - btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) -{ - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - btBroadphasePair tmpPair(*proxy0,*proxy1); - int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); - - if (findIndex < m_overlappingPairArray.size()) - { - //btAssert(it != m_overlappingPairSet.end()); - btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; - return pair; - } - return 0; -} - - - - - - - - - - -//#include - -void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) -{ - - int i; - - for (i=0;iprocessOverlap(*pair)) - { - cleanOverlappingPair(*pair,dispatcher); - pair->m_pProxy0 = 0; - pair->m_pProxy1 = 0; - m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); - m_overlappingPairArray.pop_back(); - gOverlappingPairs--; - } else - { - i++; - } - } -} - - - - -btSortedOverlappingPairCache::btSortedOverlappingPairCache(): - m_blockedForChanges(false), - m_hasDeferredRemoval(true), - m_overlapFilterCallback(0), - m_ghostPairCallback(0) -{ - int initialAllocatedSize= 2; - m_overlappingPairArray.reserve(initialAllocatedSize); -} - -btSortedOverlappingPairCache::~btSortedOverlappingPairCache() -{ -} - -void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) -{ - if (pair.m_algorithm) - { - { - pair.m_algorithm->~btCollisionAlgorithm(); - dispatcher->freeCollisionAlgorithm(pair.m_algorithm); - pair.m_algorithm=0; - gRemovePairs--; - } - } -} - - -void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class CleanPairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_cleanProxy; - btOverlappingPairCache* m_pairCache; - btDispatcher* m_dispatcher; - - public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) - :m_cleanProxy(cleanProxy), - m_pairCache(pairCache), - m_dispatcher(dispatcher) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - if ((pair.m_pProxy0 == m_cleanProxy) || - (pair.m_pProxy1 == m_cleanProxy)) - { - m_pairCache->cleanOverlappingPair(pair,m_dispatcher); - } - return false; - } - - }; - - CleanPairCallback cleanPairs(proxy,this,dispatcher); - - processAllOverlappingPairs(&cleanPairs,dispatcher); - -} - - -void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) -{ - - class RemovePairCallback : public btOverlapCallback - { - btBroadphaseProxy* m_obsoleteProxy; - - public: - RemovePairCallback(btBroadphaseProxy* obsoleteProxy) - :m_obsoleteProxy(obsoleteProxy) - { - } - virtual bool processOverlap(btBroadphasePair& pair) - { - return ((pair.m_pProxy0 == m_obsoleteProxy) || - (pair.m_pProxy1 == m_obsoleteProxy)); - } - - }; - - RemovePairCallback removeCallback(proxy); - - processAllOverlappingPairs(&removeCallback,dispatcher); -} - -void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) -{ - //should already be sorted -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btOverlappingPairCache.h" + +#include "btDispatcher.h" +#include "btCollisionAlgorithm.h" +#include "LinearMath/btAabbUtil2.h" + +#include + +int gOverlappingPairs = 0; + +int gRemovePairs =0; +int gAddedPairs =0; +int gFindPairs =0; + + + + +btHashedOverlappingPairCache::btHashedOverlappingPairCache(): + m_overlapFilterCallback(0), + m_blockedForChanges(false), + m_ghostPairCallback(0) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedOverlappingPairCache::~btHashedOverlappingPairCache() +{ +} + + + +void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +{ + if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + } + } +} + + + + +void btHashedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class CleanPairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_cleanProxy; + btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + + public: + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.m_pProxy0 == m_cleanProxy) || + (pair.m_pProxy1 == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + + + +void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class RemovePairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_obsoleteProxy; + + public: + RemovePairCallback(btBroadphaseProxy* obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.m_pProxy0 == m_obsoleteProxy) || + (pair.m_pProxy1 == m_obsoleteProxy)); + } + + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + + + + + +btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) +{ + gFindPairs++; + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if (index == BT_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include + +void btHashedOverlappingPairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_NULL_PAIR; + } + + for(i=0;igetUid(); + int proxyId2 = pair.m_pProxy1->getUid(); + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + int hashValue = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) +{ + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair != NULL) + { + return pair; + } + /*for(int i=0;i%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expand(); + + //this is where we add an actual pair, so also call the 'ghost' + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btBroadphasePair(*proxy0,*proxy1); +// pair->m_pProxy0 = proxy0; +// pair->m_pProxy1 = proxy1; + pair->m_algorithm = 0; + pair->m_internalTmpValue = 0; + + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) +{ + gRemovePairs++; + if(proxy0->m_uniqueId>proxy1->m_uniqueId) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair == NULL) + { + return 0; + } + + cleanOverlappingPair(*pair,dispatcher); + + void* userData = pair->m_internalInfo1; + + btAssert(pair->m_pProxy0->getUid() == proxyId1); + btAssert(pair->m_pProxy1->getUid() == proxyId2); + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_NULL_PAIR); + + int previous = BT_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + if (m_ghostPairCallback) + m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return userData; + } + + // Remove the last pair from the hash table. + const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->m_pProxy0->getUid()), static_cast(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_NULL_PAIR); + + previous = BT_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return userData; +} +//#include + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + +// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); + for (i=0;iprocessOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); + + gOverlappingPairs--; + } else + { + i++; + } + } +} + +void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + ///need to keep hashmap in sync with pair address, so rebuild all + btBroadphasePairArray tmpPairs; + int i; + for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + return userData; + } + } + + return 0; +} + + + + + + + + +btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + //don't add overlap with own + btAssert(proxy0 != proxy1); + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + void* mem = &m_overlappingPairArray.expand(); + btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); + + gOverlappingPairs++; + gAddedPairs++; + + if (m_ghostPairCallback) + m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); + return pair; + +} + +///this findPair becomes really slow. Either sort the list to speedup the query, or +///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. +///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) +///Also we can use a 2D bitmap, which can be useful for a future GPU implementation + btBroadphasePair* btSortedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + btBroadphasePair tmpPair(*proxy0,*proxy1); + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); + + if (findIndex < m_overlappingPairArray.size()) + { + //btAssert(it != m_overlappingPairSet.end()); + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; +} + + + + + + + + + + +//#include + +void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + + for (i=0;iprocessOverlap(*pair)) + { + cleanOverlappingPair(*pair,dispatcher); + pair->m_pProxy0 = 0; + pair->m_pProxy1 = 0; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); + gOverlappingPairs--; + } else + { + i++; + } + } +} + + + + +btSortedOverlappingPairCache::btSortedOverlappingPairCache(): + m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0), + m_ghostPairCallback(0) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); +} + +btSortedOverlappingPairCache::~btSortedOverlappingPairCache() +{ +} + +void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +{ + if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + gRemovePairs--; + } + } +} + + +void btSortedOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class CleanPairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_cleanProxy; + btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + + public: + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.m_pProxy0 == m_cleanProxy) || + (pair.m_pProxy1 == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + +void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) +{ + + class RemovePairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_obsoleteProxy; + + public: + RemovePairCallback(btBroadphaseProxy* obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.m_pProxy0 == m_obsoleteProxy) || + (pair.m_pProxy1 == m_obsoleteProxy)); + } + + }; + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + +void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + //should already be sorted +} + diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 7bed273b2..eda45c47b 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -1,468 +1,468 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef OVERLAPPING_PAIR_CACHE_H -#define OVERLAPPING_PAIR_CACHE_H - - -#include "btBroadphaseInterface.h" -#include "btBroadphaseProxy.h" -#include "btOverlappingPairCallback.h" - -#include "LinearMath/btAlignedObjectArray.h" -class btDispatcher; - -typedef btAlignedObjectArray btBroadphasePairArray; - -struct btOverlapCallback -{ - virtual ~btOverlapCallback() - {} - //return true for deletion of the pair - virtual bool processOverlap(btBroadphasePair& pair) = 0; - -}; - -struct btOverlapFilterCallback -{ - virtual ~btOverlapFilterCallback() - {} - // return true when pairs need collision - virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; -}; - - - - - - - -extern int gRemovePairs; -extern int gAddedPairs; -extern int gFindPairs; - -const int BT_NULL_PAIR=0xffffffff; - -///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. -///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations. -class btOverlappingPairCache : public btOverlappingPairCallback -{ -public: - virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor - - virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; - - virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; - - virtual btBroadphasePairArray& getOverlappingPairArray() = 0; - - virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; - - virtual int getNumOverlappingPairs() const = 0; - - virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; - - virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; - - virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; - - virtual bool hasDeferredRemoval() = 0; - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; - - -}; - -/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com -class btHashedOverlappingPairCache : public btOverlappingPairCache -{ - btBroadphasePairArray m_overlappingPairArray; - btOverlapFilterCallback* m_overlapFilterCallback; - bool m_blockedForChanges; - - -public: - btHashedOverlappingPairCache(); - virtual ~btHashedOverlappingPairCache(); - - - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - - SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); - - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } - - // Add a pair and return the new pair. If the pair already exists, - // no new pair is created and the old one is returned. - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - gAddedPairs++; - - if (!needsBroadphaseCollision(proxy0,proxy1)) - return 0; - - return internalAddPair(proxy0,proxy1); - } - - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - - virtual btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); - - int GetCount() const { return m_overlappingPairArray.size(); } -// btBroadphasePair* GetPairs() { return m_pairs; } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } - - void setOverlapFilterCallback(btOverlapFilterCallback* callback) - { - m_overlapFilterCallback = callback; - } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } -private: - - btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - void growTables(); - - SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) - { - return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2; - } - - /* - // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm - // This assumes proxyId1 and proxyId2 are 16-bit. - SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2) - { - int key = (proxyId2 << 16) | proxyId1; - key = ~key + (key << 15); - key = key ^ (key >> 12); - key = key + (key << 2); - key = key ^ (key >> 4); - key = key * 2057; - key = key ^ (key >> 16); - return key; - } - */ - - - - SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) - { - int key = static_cast(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); - // Thomas Wang's hash - - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); - return static_cast(key); - } - - - - - - SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) - { - int proxyId1 = proxy0->getUid(); - int proxyId2 = proxy1->getUid(); - #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. - if (proxyId1 > proxyId2) - btSwap(proxyId1, proxyId2); - #endif - - int index = m_hashTable[hash]; - - while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) - { - index = m_next[index]; - } - - if ( index == BT_NULL_PAIR ) - { - return NULL; - } - - btAssert(index < m_overlappingPairArray.size()); - - return &m_overlappingPairArray[index]; - } - - virtual bool hasDeferredRemoval() - { - return false; - } - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) - { - m_ghostPairCallback = ghostPairCallback; - } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - - -protected: - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - btOverlappingPairCallback* m_ghostPairCallback; - -}; - - - - -///btSortedOverlappingPairCache maintains the objects with overlapping AABB -///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase -class btSortedOverlappingPairCache : public btOverlappingPairCache -{ - protected: - //avoid brute-force finding all the time - btBroadphasePairArray m_overlappingPairArray; - - //during the dispatch, check that user doesn't destroy/create proxy - bool m_blockedForChanges; - - ///by default, do the removal during the pair traversal - bool m_hasDeferredRemoval; - - //if set, use the callback instead of the built in filter in needBroadphaseCollision - btOverlapFilterCallback* m_overlapFilterCallback; - - btOverlappingPairCallback* m_ghostPairCallback; - - public: - - btSortedOverlappingPairCache(); - virtual ~btSortedOverlappingPairCache(); - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - - void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); - - void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - - btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - - - void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - - - inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const - { - if (m_overlapFilterCallback) - return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); - - bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; - collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - - return collides; - } - - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - const btBroadphasePairArray& getOverlappingPairArray() const - { - return m_overlappingPairArray; - } - - - - - btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - - int getNumOverlappingPairs() const - { - return m_overlappingPairArray.size(); - } - - btOverlapFilterCallback* getOverlapFilterCallback() - { - return m_overlapFilterCallback; - } - - void setOverlapFilterCallback(btOverlapFilterCallback* callback) - { - m_overlapFilterCallback = callback; - } - - virtual bool hasDeferredRemoval() - { - return m_hasDeferredRemoval; - } - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) - { - m_ghostPairCallback = ghostPairCallback; - } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher); - - -}; - - - -///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. -class btNullPairCache : public btOverlappingPairCache -{ - - btBroadphasePairArray m_overlappingPairArray; - -public: - - virtual btBroadphasePair* getOverlappingPairArrayPtr() - { - return &m_overlappingPairArray[0]; - } - const btBroadphasePair* getOverlappingPairArrayPtr() const - { - return &m_overlappingPairArray[0]; - } - btBroadphasePairArray& getOverlappingPairArray() - { - return m_overlappingPairArray; - } - - virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) - { - - } - - virtual int getNumOverlappingPairs() const - { - return 0; - } - - virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) - { - - } - - virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) - { - } - - virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) - { - } - - virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) - { - return 0; - } - - virtual bool hasDeferredRemoval() - { - return true; - } - - virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) - { - - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) - { - return 0; - } - - virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) - { - return 0; - } - - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) - { - } - - virtual void sortOverlappingPairs(btDispatcher* dispatcher) - { - } - - -}; - - -#endif //OVERLAPPING_PAIR_CACHE_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef OVERLAPPING_PAIR_CACHE_H +#define OVERLAPPING_PAIR_CACHE_H + + +#include "btBroadphaseInterface.h" +#include "btBroadphaseProxy.h" +#include "btOverlappingPairCallback.h" + +#include "LinearMath/btAlignedObjectArray.h" +class btDispatcher; + +typedef btAlignedObjectArray btBroadphasePairArray; + +struct btOverlapCallback +{ + virtual ~btOverlapCallback() + {} + //return true for deletion of the pair + virtual bool processOverlap(btBroadphasePair& pair) = 0; + +}; + +struct btOverlapFilterCallback +{ + virtual ~btOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; +}; + + + + + + + +extern int gRemovePairs; +extern int gAddedPairs; +extern int gFindPairs; + +const int BT_NULL_PAIR=0xffffffff; + +///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. +///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations. +class btOverlappingPairCache : public btOverlappingPairCallback +{ +public: + virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor + + virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; + + virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; + + virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; + + virtual int getNumOverlappingPairs() const = 0; + + virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + + virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; + + virtual bool hasDeferredRemoval() = 0; + + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; + + +}; + +/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com +class btHashedOverlappingPairCache : public btOverlappingPairCache +{ + btBroadphasePairArray m_overlappingPairArray; + btOverlapFilterCallback* m_overlapFilterCallback; + bool m_blockedForChanges; + + +public: + btHashedOverlappingPairCache(); + virtual ~btHashedOverlappingPairCache(); + + + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); + + SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + gAddedPairs++; + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + return internalAddPair(proxy0,proxy1); + } + + + + void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + + + + btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); + + int GetCount() const { return m_overlappingPairArray.size(); } +// btBroadphasePair* GetPairs() { return m_pairs; } + + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) + { + return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == proxyId2; + } + + /* + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2) + { + int key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + */ + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + { + int key = static_cast(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast(key); + } + + + + + + SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) + { + int proxyId1 = proxy0->getUid(); + int proxyId2 = proxy1->getUid(); + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. + if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2); + #endif + + int index = m_hashTable[hash]; + + while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if ( index == BT_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + virtual bool hasDeferredRemoval() + { + return false; + } + + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +protected: + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + btOverlappingPairCallback* m_ghostPairCallback; + +}; + + + + +///btSortedOverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase +class btSortedOverlappingPairCache : public btOverlappingPairCache +{ + protected: + //avoid brute-force finding all the time + btBroadphasePairArray m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; + + //if set, use the callback instead of the built in filter in needBroadphaseCollision + btOverlapFilterCallback* m_overlapFilterCallback; + + btOverlappingPairCallback* m_ghostPairCallback; + + public: + + btSortedOverlappingPairCache(); + virtual ~btSortedOverlappingPairCache(); + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + + void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); + + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + + btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + + void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + + + inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + + + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +}; + + + +///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. +class btNullPairCache : public btOverlappingPairCache +{ + + btBroadphasePairArray m_overlappingPairArray; + +public: + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual int getNumOverlappingPairs() const + { + return 0; + } + + virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) + { + } + + virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) + { + return 0; + } + + virtual bool hasDeferredRemoval() + { + return true; + } + + virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) + { + return 0; + } + + virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) + { + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) + { + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) + { + } + + +}; + + +#endif //OVERLAPPING_PAIR_CACHE_H + + diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp index f4f1753c7..8bef8f0d4 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp @@ -1,1148 +1,1148 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btQuantizedBvh.h" - -#include "LinearMath/btAabbUtil2.h" -#include "LinearMath/btIDebugDraw.h" - -#define RAYAABB2 - -btQuantizedBvh::btQuantizedBvh() : - m_bulletVersion(BT_BULLET_VERSION), - m_useQuantization(false), - //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - m_traversalMode(TRAVERSAL_STACKLESS) - //m_traversalMode(TRAVERSAL_RECURSIVE) - ,m_subtreeHeaderCount(0) //PCK: add this line -{ - m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); - m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); -} - - - - - -void btQuantizedBvh::buildInternal() -{ - ///assumes that caller filled in the m_quantizedLeafNodes - m_useQuantization = true; - int numLeafNodes = 0; - - if (m_useQuantization) - { - //now we have an array of leafnodes in m_leafNodes - numLeafNodes = m_quantizedLeafNodes.size(); - - m_quantizedContiguousNodes.resize(2*numLeafNodes); - - } - - m_curNodeIndex = 0; - - buildTree(0,numLeafNodes); - - ///if the entire tree is small then subtree size, we need to create a header info for the tree - if(m_useQuantization && !m_SubtreeHeaders.size()) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); - subtree.m_rootNodeIndex = 0; - subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); - } - - //PCK: update the copy of the size - m_subtreeHeaderCount = m_SubtreeHeaders.size(); - - //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary - m_quantizedLeafNodes.clear(); - m_leafNodes.clear(); -} - - - -///just for debugging, to visualize the individual patches/subtrees -#ifdef DEBUG_PATCH_COLORS -btVector3 color[4]= -{ - btVector3(255,0,0), - btVector3(0,255,0), - btVector3(0,0,255), - btVector3(0,255,255) -}; -#endif //DEBUG_PATCH_COLORS - - - -void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) -{ - //enlarge the AABB to avoid division by zero when initializing the quantization values - btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); - m_bvhAabbMin = bvhAabbMin - clampValue; - m_bvhAabbMax = bvhAabbMax + clampValue; - btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; - m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; - m_useQuantization = true; -} - - - - -btQuantizedBvh::~btQuantizedBvh() -{ -} - -#ifdef DEBUG_TREE_BUILDING -int gStackDepth = 0; -int gMaxStackDepth = 0; -#endif //DEBUG_TREE_BUILDING - -void btQuantizedBvh::buildTree (int startIndex,int endIndex) -{ -#ifdef DEBUG_TREE_BUILDING - gStackDepth++; - if (gStackDepth > gMaxStackDepth) - gMaxStackDepth = gStackDepth; -#endif //DEBUG_TREE_BUILDING - - - int splitAxis, splitIndex, i; - int numIndices =endIndex-startIndex; - int curIndex = m_curNodeIndex; - - btAssert(numIndices>0); - - if (numIndices==1) - { -#ifdef DEBUG_TREE_BUILDING - gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); - - m_curNodeIndex++; - return; - } - //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. - - splitAxis = calcSplittingAxis(startIndex,endIndex); - - splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); - - int internalNodeIndex = m_curNodeIndex; - - //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. - //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values - setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization - setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization - - - for (i=startIndex;im_escapeIndex; - - int leftChildNodexIndex = m_curNodeIndex; - - //build left child tree - buildTree(startIndex,splitIndex); - - int rightChildNodexIndex = m_curNodeIndex; - //build right child tree - buildTree(splitIndex,endIndex); - -#ifdef DEBUG_TREE_BUILDING - gStackDepth--; -#endif //DEBUG_TREE_BUILDING - - int escapeIndex = m_curNodeIndex - curIndex; - - if (m_useQuantization) - { - //escapeIndex is the number of nodes of this subtree - const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); - const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; - if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) - { - updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); - } - } else - { - - } - - setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); - -} - -void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) -{ - btAssert(m_useQuantization); - - btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; - int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); - int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - - btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; - int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); - int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); - - if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(leftChildNode); - subtree.m_rootNodeIndex = leftChildNodexIndex; - subtree.m_subtreeSize = leftSubTreeSize; - } - - if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) - { - btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); - subtree.setAabbFromQuantizeNode(rightChildNode); - subtree.m_rootNodeIndex = rightChildNodexIndex; - subtree.m_subtreeSize = rightSubTreeSize; - } - - //PCK: update the copy of the size - m_subtreeHeaderCount = m_SubtreeHeaders.size(); -} - - -int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) -{ - int i; - int splitIndex =startIndex; - int numIndices = endIndex - startIndex; - btScalar splitValue; - - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - for (i=startIndex;i splitValue) - { - //swap - swapLeafNodes(i,splitIndex); - splitIndex++; - } - } - - //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex - //otherwise the tree-building might fail due to stack-overflows in certain cases. - //unbalanced1 is unsafe: it can cause stack overflows - //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); - - //unbalanced2 should work too: always use center (perfect balanced trees) - //bool unbalanced2 = true; - - //this should be safe too: - int rangeBalancedIndices = numIndices/3; - bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); - - if (unbalanced) - { - splitIndex = startIndex+ (numIndices>>1); - } - - bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); - (void)unbal; - btAssert(!unbal); - - return splitIndex; -} - - -int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) -{ - int i; - - btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); - btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); - int numIndices = endIndex-startIndex; - - for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); - isLeafNode = rootNode->m_escapeIndex == -1; - - //PCK: unsigned instead of bool - if (isLeafNode && (aabbOverlap != 0)) - { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - - //PCK: unsigned instead of bool - if ((aabbOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->m_escapeIndex; - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -/* -///this was the original recursive traversal, before we optimized towards stackless traversal -void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); - if (aabbOverlap) - { - isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); - if (isLeafNode) - { - nodeCallback->processNode(rootNode); - } else - { - walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); - walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); - } - } - -} -*/ - -void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const -{ - btAssert(m_useQuantization); - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap; - - //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); - isLeafNode = currentNode->isLeafNode(); - - //PCK: unsigned instead of bool - if (aabbOverlap != 0) - { - if (isLeafNode) - { - nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); - } else - { - //process left and right children - const btQuantizedBvhNode* leftChildNode = currentNode+1; - walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); - - const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); - walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); - } - } -} - - - -void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const -{ - btAssert(!m_useQuantization); - - const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; - int escapeIndex, curIndex = 0; - int walkIterations = 0; - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap=0; - unsigned rayBoxOverlap=0; - btScalar lambda_max = 1.0; - - /* Quick pruning by quantized box */ - btVector3 rayAabbMin = raySource; - btVector3 rayAabbMax = raySource; - rayAabbMin.setMin(rayTarget); - rayAabbMax.setMax(rayTarget); - - /* Add box cast extents to bounding box */ - rayAabbMin += aabbMin; - rayAabbMax += aabbMax; - -#ifdef RAYAABB2 - btVector3 rayDir = (rayTarget-raySource); - rayDir.normalize (); - lambda_max = rayDir.dot(rayTarget-raySource); - ///what about division by zero? --> just set rayDirection[i] to 1.0 - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; -#endif - - btVector3 bounds[2]; - - while (curIndex < m_curNodeIndex) - { - btScalar param = 1.0; - //catch bugs in tree data - btAssert (walkIterations < m_curNodeIndex); - - walkIterations++; - - bounds[0] = rootNode->m_aabbMinOrg; - bounds[1] = rootNode->m_aabbMaxOrg; - /* Add box cast extents */ - bounds[0] += aabbMin; - bounds[1] += aabbMax; - - aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); - //perhaps profile if it is worth doing the aabbOverlap test first - -#ifdef RAYAABB2 - ///careful with this check: need to check division by zero (above) and fix the unQuantize method - ///thanks Joerg/hiker for the reproduction case! - ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; - -#else - btVector3 normal; - rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); -#endif - - isLeafNode = rootNode->m_escapeIndex == -1; - - //PCK: unsigned instead of bool - if (isLeafNode && (rayBoxOverlap != 0)) - { - nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); - } - - //PCK: unsigned instead of bool - if ((rayBoxOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->m_escapeIndex; - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - - - -void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const -{ - btAssert(m_useQuantization); - - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - (void)subTreeSize; - - const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; - int escapeIndex; - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned boxBoxOverlap = 0; - unsigned rayBoxOverlap = 0; - - btScalar lambda_max = 1.0; - -#ifdef RAYAABB2 - btVector3 rayDirection = (rayTarget-raySource); - rayDirection.normalize (); - lambda_max = rayDirection.dot(rayTarget-raySource); - ///what about division by zero? --> just set rayDirection[i] to 1.0 - rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; - rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; - rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; - unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; -#endif - - /* Quick pruning by quantized box */ - btVector3 rayAabbMin = raySource; - btVector3 rayAabbMax = raySource; - rayAabbMin.setMin(rayTarget); - rayAabbMax.setMax(rayTarget); - - /* Add box cast extents to bounding box */ - rayAabbMin += aabbMin; - rayAabbMax += aabbMax; - - unsigned short int quantizedQueryAabbMin[3]; - unsigned short int quantizedQueryAabbMax[3]; - quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); - quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); - - while (curIndex < endNodeIndex) - { - -//#define VISUALLY_ANALYZE_BVH 1 -#ifdef VISUALLY_ANALYZE_BVH - //some code snippet to debugDraw aabb, to visually analyze bvh structure - static int drawPatch = 0; - //need some global access to a debugDrawer - extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) - { - btVector3 aabbMin,aabbMax; - aabbMin = unQuantize(rootNode->m_quantizedAabbMin); - aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); - } -#endif//VISUALLY_ANALYZE_BVH - - //catch bugs in tree data - btAssert (walkIterations < subTreeSize); - - walkIterations++; - //PCK: unsigned instead of bool - // only interested if this is closer than any previous hit - btScalar param = 1.0; - rayBoxOverlap = 0; - boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - if (boxBoxOverlap) - { - btVector3 bounds[2]; - bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); - bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); - /* Add box cast extents */ - bounds[0] += aabbMin; - bounds[1] += aabbMax; - btVector3 normal; -#if 0 - bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); - bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); - if (ra2 != ra) - { - printf("functions don't match\n"); - } -#endif -#ifdef RAYAABB2 - ///careful with this check: need to check division by zero (above) and fix the unQuantize method - ///thanks Joerg/hiker for the reproduction case! - ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 - - //BT_PROFILE("btRayAabb2"); - rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); - -#else - rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); -#endif - } - - if (isLeafNode && rayBoxOverlap) - { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - - //PCK: unsigned instead of bool - if ((rayBoxOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const -{ - btAssert(m_useQuantization); - - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - (void)subTreeSize; - - const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; - int escapeIndex; - - bool isLeafNode; - //PCK: unsigned instead of bool - unsigned aabbOverlap; - - while (curIndex < endNodeIndex) - { - -//#define VISUALLY_ANALYZE_BVH 1 -#ifdef VISUALLY_ANALYZE_BVH - //some code snippet to debugDraw aabb, to visually analyze bvh structure - static int drawPatch = 0; - //need some global access to a debugDrawer - extern btIDebugDraw* debugDrawerPtr; - if (curIndex==drawPatch) - { - btVector3 aabbMin,aabbMax; - aabbMin = unQuantize(rootNode->m_quantizedAabbMin); - aabbMax = unQuantize(rootNode->m_quantizedAabbMax); - btVector3 color(1,0,0); - debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); - } -#endif//VISUALLY_ANALYZE_BVH - - //catch bugs in tree data - btAssert (walkIterations < subTreeSize); - - walkIterations++; - //PCK: unsigned instead of bool - aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - - if (isLeafNode && aabbOverlap) - { - nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); - } - - //PCK: unsigned instead of bool - if ((aabbOverlap != 0) || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - if (maxIterations < walkIterations) - maxIterations = walkIterations; - -} - -//This traversal can be called from Playstation 3 SPU -void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const -{ - btAssert(m_useQuantization); - - int i; - - - for (i=0;im_SubtreeHeaders.size();i++) - { - const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - - //PCK: unsigned instead of bool - unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap != 0) - { - walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - subtree.m_rootNodeIndex, - subtree.m_rootNodeIndex+subtree.m_subtreeSize); - } - } -} - - -void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const -{ - reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); -} - - -void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const -{ - //always use stackless - - if (m_useQuantization) - { - walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } - else - { - walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); - } - /* - { - //recursive traversal - btVector3 qaabbMin = raySource; - btVector3 qaabbMax = raySource; - qaabbMin.setMin(rayTarget); - qaabbMax.setMax(rayTarget); - qaabbMin += aabbMin; - qaabbMax += aabbMax; - reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); - } - */ - -} - - -void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) -{ - if (m_useQuantization) - { - btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; - m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; - m_quantizedLeafNodes[splitIndex] = tmp; - } else - { - btOptimizedBvhNode tmp = m_leafNodes[i]; - m_leafNodes[i] = m_leafNodes[splitIndex]; - m_leafNodes[splitIndex] = tmp; - } -} - -void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) -{ - if (m_useQuantization) - { - m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; - } else - { - m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; - } -} - -//PCK: include -#include - -#if 0 -//PCK: consts -static const unsigned BVH_ALIGNMENT = 16; -static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; - -static const unsigned BVH_ALIGNMENT_BLOCKS = 2; -#endif - - -unsigned int btQuantizedBvh::getAlignmentSerializationPadding() -{ - // I changed this to 0 since the extra padding is not needed or used. - return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; -} - -unsigned btQuantizedBvh::calculateSerializeBufferSize() -{ - unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); - baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; - if (m_useQuantization) - { - return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode); - } - return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); -} - -bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) -{ - btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); - m_subtreeHeaderCount = m_SubtreeHeaders.size(); - -/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) - { - ///check alignedment for buffer? - btAssert(0); - return false; - } -*/ - - btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; - - // construct the class so the virtual function table, etc will be set up - // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor - new (targetBvh) btQuantizedBvh; - - if (i_swapEndian) - { - targetBvh->m_curNodeIndex = static_cast(btSwapEndian(m_curNodeIndex)); - - - btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); - btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); - btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); - - targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); - targetBvh->m_subtreeHeaderCount = static_cast(btSwapEndian(m_subtreeHeaderCount)); - } - else - { - targetBvh->m_curNodeIndex = m_curNodeIndex; - targetBvh->m_bvhAabbMin = m_bvhAabbMin; - targetBvh->m_bvhAabbMax = m_bvhAabbMax; - targetBvh->m_bvhQuantization = m_bvhQuantization; - targetBvh->m_traversalMode = m_traversalMode; - targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount; - } - - targetBvh->m_useQuantization = m_useQuantization; - - unsigned char *nodeData = (unsigned char *)targetBvh; - nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - int nodeCount = m_curNodeIndex; - - if (m_useQuantization) - { - targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); - } - } - else - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]; - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]; - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; - - targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; - - - } - } - nodeData += sizeof(btQuantizedBvhNode) * nodeCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0); - } - else - { - targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); - btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); - - targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); - targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); - targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex)); - } - } - else - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg; - targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg; - - targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex; - targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart; - targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex; - } - } - nodeData += sizeof(btOptimizedBvhNode) * nodeCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); - } - - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - // Now serialize the subtree headers - targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount); - if (i_swapEndian) - { - for (int i = 0; i < m_subtreeHeaderCount; i++) - { - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); - targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize)); - } - } - else - { - for (int i = 0; i < m_subtreeHeaderCount; i++) - { - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); - targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); - - // need to clear padding in destination buffer - targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0; - targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0; - targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0; - } - } - nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; - - // this clears the pointer in the member variable it doesn't really do anything to the data - // it does call the destructor on the contained objects, but they are all classes with no destructor defined - // so the memory (which is not freed) is left alone - targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0); - - // this wipes the virtual function table pointer at the start of the buffer for the class - *((void**)o_alignedDataBuffer) = NULL; - - return true; -} - -btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) -{ - - if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) - { - return NULL; - } - btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; - - if (i_swapEndian) - { - bvh->m_curNodeIndex = static_cast(btSwapEndian(bvh->m_curNodeIndex)); - - btUnSwapVector3Endian(bvh->m_bvhAabbMin); - btUnSwapVector3Endian(bvh->m_bvhAabbMax); - btUnSwapVector3Endian(bvh->m_bvhQuantization); - - bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode); - bvh->m_subtreeHeaderCount = static_cast(btSwapEndian(bvh->m_subtreeHeaderCount)); - } - - unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); - btAssert(calculatedBufSize <= i_dataBufferSize); - - if (calculatedBufSize > i_dataBufferSize) - { - return NULL; - } - - unsigned char *nodeData = (unsigned char *)bvh; - nodeData += sizeof(btQuantizedBvh); - - unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - int nodeCount = bvh->m_curNodeIndex; - - // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor - // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor - new (bvh) btQuantizedBvh(*bvh, false); - - if (bvh->m_useQuantization) - { - bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); - - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); - bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); - - bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); - } - } - nodeData += sizeof(btQuantizedBvhNode) * nodeCount; - } - else - { - bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); - - if (i_swapEndian) - { - for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) - { - btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); - btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); - - bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); - bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); - bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); - } - } - nodeData += sizeof(btOptimizedBvhNode) * nodeCount; - } - - sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; - nodeData += sizeToAdd; - - // Now serialize the subtree headers - bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount); - if (i_swapEndian) - { - for (int i = 0; i < bvh->m_subtreeHeaderCount; i++) - { - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); - - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); - bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); - - bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); - bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); - } - } - - return bvh; -} - -// Constructor that prevents btVector3's default constructor from being called -btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : -m_bvhAabbMin(self.m_bvhAabbMin), -m_bvhAabbMax(self.m_bvhAabbMax), -m_bvhQuantization(self.m_bvhQuantization), -m_bulletVersion(BT_BULLET_VERSION) -{ - -} - - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btQuantizedBvh.h" + +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btIDebugDraw.h" + +#define RAYAABB2 + +btQuantizedBvh::btQuantizedBvh() : + m_bulletVersion(BT_BULLET_VERSION), + m_useQuantization(false), + //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) + m_traversalMode(TRAVERSAL_STACKLESS) + //m_traversalMode(TRAVERSAL_RECURSIVE) + ,m_subtreeHeaderCount(0) //PCK: add this line +{ + m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY); + m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); +} + + + + + +void btQuantizedBvh::buildInternal() +{ + ///assumes that caller filled in the m_quantizedLeafNodes + m_useQuantization = true; + int numLeafNodes = 0; + + if (m_useQuantization) + { + //now we have an array of leafnodes in m_leafNodes + numLeafNodes = m_quantizedLeafNodes.size(); + + m_quantizedContiguousNodes.resize(2*numLeafNodes); + + } + + m_curNodeIndex = 0; + + buildTree(0,numLeafNodes); + + ///if the entire tree is small then subtree size, we need to create a header info for the tree + if(m_useQuantization && !m_SubtreeHeaders.size()) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[0]); + subtree.m_rootNodeIndex = 0; + subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + + //PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary + m_quantizedLeafNodes.clear(); + m_leafNodes.clear(); +} + + + +///just for debugging, to visualize the individual patches/subtrees +#ifdef DEBUG_PATCH_COLORS +btVector3 color[4]= +{ + btVector3(255,0,0), + btVector3(0,255,0), + btVector3(0,0,255), + btVector3(0,255,255) +}; +#endif //DEBUG_PATCH_COLORS + + + +void btQuantizedBvh::setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin) +{ + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + m_bvhAabbMin = bvhAabbMin - clampValue; + m_bvhAabbMax = bvhAabbMax + clampValue; + btVector3 aabbSize = m_bvhAabbMax - m_bvhAabbMin; + m_bvhQuantization = btVector3(btScalar(65533.0),btScalar(65533.0),btScalar(65533.0)) / aabbSize; + m_useQuantization = true; +} + + + + +btQuantizedBvh::~btQuantizedBvh() +{ +} + +#ifdef DEBUG_TREE_BUILDING +int gStackDepth = 0; +int gMaxStackDepth = 0; +#endif //DEBUG_TREE_BUILDING + +void btQuantizedBvh::buildTree (int startIndex,int endIndex) +{ +#ifdef DEBUG_TREE_BUILDING + gStackDepth++; + if (gStackDepth > gMaxStackDepth) + gMaxStackDepth = gStackDepth; +#endif //DEBUG_TREE_BUILDING + + + int splitAxis, splitIndex, i; + int numIndices =endIndex-startIndex; + int curIndex = m_curNodeIndex; + + btAssert(numIndices>0); + + if (numIndices==1) + { +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(m_curNodeIndex,startIndex); + + m_curNodeIndex++; + return; + } + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + splitAxis = calcSplittingAxis(startIndex,endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex,endIndex,splitAxis); + + int internalNodeIndex = m_curNodeIndex; + + //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value. + //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values + setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization + setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization + + + for (i=startIndex;im_escapeIndex; + + int leftChildNodexIndex = m_curNodeIndex; + + //build left child tree + buildTree(startIndex,splitIndex); + + int rightChildNodexIndex = m_curNodeIndex; + //build right child tree + buildTree(splitIndex,endIndex); + +#ifdef DEBUG_TREE_BUILDING + gStackDepth--; +#endif //DEBUG_TREE_BUILDING + + int escapeIndex = m_curNodeIndex - curIndex; + + if (m_useQuantization) + { + //escapeIndex is the number of nodes of this subtree + const int sizeQuantizedNode =sizeof(btQuantizedBvhNode); + const int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) + { + updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex); + } + } else + { + + } + + setInternalNodeEscapeIndex(internalNodeIndex,escapeIndex); + +} + +void btQuantizedBvh::updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex) +{ + btAssert(m_useQuantization); + + btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = leftChildNode.isLeafNode() ? 1 : leftChildNode.getEscapeIndex(); + int leftSubTreeSizeInBytes = leftSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + + btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = rightChildNode.isLeafNode() ? 1 : rightChildNode.getEscapeIndex(); + int rightSubTreeSizeInBytes = rightSubTreeSize * static_cast(sizeof(btQuantizedBvhNode)); + + if(leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(leftChildNode); + subtree.m_rootNodeIndex = leftChildNodexIndex; + subtree.m_subtreeSize = leftSubTreeSize; + } + + if(rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) + { + btBvhSubtreeInfo& subtree = m_SubtreeHeaders.expand(); + subtree.setAabbFromQuantizeNode(rightChildNode); + subtree.m_rootNodeIndex = rightChildNodexIndex; + subtree.m_subtreeSize = rightSubTreeSize; + } + + //PCK: update the copy of the size + m_subtreeHeaderCount = m_SubtreeHeaders.size(); +} + + +int btQuantizedBvh::sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + btScalar splitValue; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + for (i=startIndex;i splitValue) + { + //swap + swapLeafNodes(i,splitIndex); + splitIndex++; + } + } + + //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + //otherwise the tree-building might fail due to stack-overflows in certain cases. + //unbalanced1 is unsafe: it can cause stack overflows + //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + //unbalanced2 should work too: always use center (perfect balanced trees) + //bool unbalanced2 = true; + + //this should be safe too: + int rangeBalancedIndices = numIndices/3; + bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices))); + + if (unbalanced) + { + splitIndex = startIndex+ (numIndices>>1); + } + + bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex)); + (void)unbal; + btAssert(!unbal); + + return splitIndex; +} + + +int btQuantizedBvh::calcSplittingAxis(int startIndex,int endIndex) +{ + int i; + + btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.)); + btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.)); + int numIndices = endIndex-startIndex; + + for (i=startIndex;im_aabbMinOrg,rootNode->m_aabbMaxOrg); + isLeafNode = rootNode->m_escapeIndex == -1; + + //PCK: unsigned instead of bool + if (isLeafNode && (aabbOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +/* +///this was the original recursive traversal, before we optimized towards stackless traversal +void btQuantizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); + if (aabbOverlap) + { + isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + if (isLeafNode) + { + nodeCallback->processNode(rootNode); + } else + { + walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); + walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); + } + } + +} +*/ + +void btQuantizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); + isLeafNode = currentNode->isLeafNode(); + + //PCK: unsigned instead of bool + if (aabbOverlap != 0) + { + if (isLeafNode) + { + nodeCallback->processNode(currentNode->getPartId(),currentNode->getTriangleIndex()); + } else + { + //process left and right children + const btQuantizedBvhNode* leftChildNode = currentNode+1; + walkRecursiveQuantizedTreeAgainstQueryAabb(leftChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + + const btQuantizedBvhNode* rightChildNode = leftChildNode->isLeafNode() ? leftChildNode+1:leftChildNode+leftChildNode->getEscapeIndex(); + walkRecursiveQuantizedTreeAgainstQueryAabb(rightChildNode,nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax); + } + } +} + + + +void btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(!m_useQuantization); + + const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0]; + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap=0; + unsigned rayBoxOverlap=0; + btScalar lambda_max = 1.0; + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + +#ifdef RAYAABB2 + btVector3 rayDir = (rayTarget-raySource); + rayDir.normalize (); + lambda_max = rayDir.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; +#endif + + btVector3 bounds[2]; + + while (curIndex < m_curNodeIndex) + { + btScalar param = 1.0; + //catch bugs in tree data + btAssert (walkIterations < m_curNodeIndex); + + walkIterations++; + + bounds[0] = rootNode->m_aabbMinOrg; + bounds[1] = rootNode->m_aabbMaxOrg; + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + + aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg); + //perhaps profile if it is worth doing the aabbOverlap test first + +#ifdef RAYAABB2 + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false; + +#else + btVector3 normal; + rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal); +#endif + + isLeafNode = rootNode->m_escapeIndex == -1; + + //PCK: unsigned instead of bool + if (isLeafNode && (rayBoxOverlap != 0)) + { + nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + + + +void btQuantizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned boxBoxOverlap = 0; + unsigned rayBoxOverlap = 0; + + btScalar lambda_max = 1.0; + +#ifdef RAYAABB2 + btVector3 rayDirection = (rayTarget-raySource); + rayDirection.normalize (); + lambda_max = rayDirection.dot(rayTarget-raySource); + ///what about division by zero? --> just set rayDirection[i] to 1.0 + rayDirection[0] = rayDirection[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[0]; + rayDirection[1] = rayDirection[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[1]; + rayDirection[2] = rayDirection[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDirection[2]; + unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; +#endif + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + /* Add box cast extents to bounding box */ + rayAabbMin += aabbMin; + rayAabbMax += aabbMax; + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin,0); + quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax,1); + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + btAssert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + // only interested if this is closer than any previous hit + btScalar param = 1.0; + rayBoxOverlap = 0; + boxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + if (boxBoxOverlap) + { + btVector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + /* Add box cast extents */ + bounds[0] += aabbMin; + bounds[1] += aabbMax; + btVector3 normal; +#if 0 + bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); + if (ra2 != ra) + { + printf("functions don't match\n"); + } +#endif +#ifdef RAYAABB2 + ///careful with this check: need to check division by zero (above) and fix the unQuantize method + ///thanks Joerg/hiker for the reproduction case! + ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858 + + //BT_PROFILE("btRayAabb2"); + rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max); + +#else + rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); +#endif + } + + if (isLeafNode && rayBoxOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((rayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +void btQuantizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + (void)subTreeSize; + + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned aabbOverlap; + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + btAssert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((aabbOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + +//This traversal can be called from Playstation 3 SPU +void btQuantizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const +{ + btAssert(m_useQuantization); + + int i; + + + for (i=0;im_SubtreeHeaders.size();i++) + { + const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; + + //PCK: unsigned instead of bool + unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + subtree.m_rootNodeIndex, + subtree.m_rootNodeIndex+subtree.m_subtreeSize); + } + } +} + + +void btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const +{ + reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0)); +} + + +void btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //always use stackless + + if (m_useQuantization) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } + else + { + walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } + /* + { + //recursive traversal + btVector3 qaabbMin = raySource; + btVector3 qaabbMax = raySource; + qaabbMin.setMin(rayTarget); + qaabbMax.setMax(rayTarget); + qaabbMin += aabbMin; + qaabbMax += aabbMax; + reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax); + } + */ + +} + + +void btQuantizedBvh::swapLeafNodes(int i,int splitIndex) +{ + if (m_useQuantization) + { + btQuantizedBvhNode tmp = m_quantizedLeafNodes[i]; + m_quantizedLeafNodes[i] = m_quantizedLeafNodes[splitIndex]; + m_quantizedLeafNodes[splitIndex] = tmp; + } else + { + btOptimizedBvhNode tmp = m_leafNodes[i]; + m_leafNodes[i] = m_leafNodes[splitIndex]; + m_leafNodes[splitIndex] = tmp; + } +} + +void btQuantizedBvh::assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex) +{ + if (m_useQuantization) + { + m_quantizedContiguousNodes[internalNode] = m_quantizedLeafNodes[leafNodeIndex]; + } else + { + m_contiguousNodes[internalNode] = m_leafNodes[leafNodeIndex]; + } +} + +//PCK: include +#include + +#if 0 +//PCK: consts +static const unsigned BVH_ALIGNMENT = 16; +static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1; + +static const unsigned BVH_ALIGNMENT_BLOCKS = 2; +#endif + + +unsigned int btQuantizedBvh::getAlignmentSerializationPadding() +{ + // I changed this to 0 since the extra padding is not needed or used. + return 0;//BVH_ALIGNMENT_BLOCKS * BVH_ALIGNMENT; +} + +unsigned btQuantizedBvh::calculateSerializeBufferSize() +{ + unsigned baseSize = sizeof(btQuantizedBvh) + getAlignmentSerializationPadding(); + baseSize += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + if (m_useQuantization) + { + return baseSize + m_curNodeIndex * sizeof(btQuantizedBvhNode); + } + return baseSize + m_curNodeIndex * sizeof(btOptimizedBvhNode); +} + +bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian) +{ + btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size()); + m_subtreeHeaderCount = m_SubtreeHeaders.size(); + +/* if (i_dataBufferSize < calculateSerializeBufferSize() || o_alignedDataBuffer == NULL || (((unsigned)o_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + ///check alignedment for buffer? + btAssert(0); + return false; + } +*/ + + btQuantizedBvh *targetBvh = (btQuantizedBvh *)o_alignedDataBuffer; + + // construct the class so the virtual function table, etc will be set up + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (targetBvh) btQuantizedBvh; + + if (i_swapEndian) + { + targetBvh->m_curNodeIndex = static_cast(btSwapEndian(m_curNodeIndex)); + + + btSwapVector3Endian(m_bvhAabbMin,targetBvh->m_bvhAabbMin); + btSwapVector3Endian(m_bvhAabbMax,targetBvh->m_bvhAabbMax); + btSwapVector3Endian(m_bvhQuantization,targetBvh->m_bvhQuantization); + + targetBvh->m_traversalMode = (btTraversalMode)btSwapEndian(m_traversalMode); + targetBvh->m_subtreeHeaderCount = static_cast(btSwapEndian(m_subtreeHeaderCount)); + } + else + { + targetBvh->m_curNodeIndex = m_curNodeIndex; + targetBvh->m_bvhAabbMin = m_bvhAabbMin; + targetBvh->m_bvhAabbMax = m_bvhAabbMax; + targetBvh->m_bvhQuantization = m_bvhQuantization; + targetBvh->m_traversalMode = m_traversalMode; + targetBvh->m_subtreeHeaderCount = m_subtreeHeaderCount; + } + + targetBvh->m_useQuantization = m_useQuantization; + + unsigned char *nodeData = (unsigned char *)targetBvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = m_curNodeIndex; + + if (m_useQuantization) + { + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]; + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]; + + targetBvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex; + + + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_quantizedContiguousNodes.initializeFromBuffer(NULL, 0, 0); + } + else + { + targetBvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMinOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btSwapVector3Endian(m_contiguousNodes[nodeIndex].m_aabbMaxOrg, targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_escapeIndex)); + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_subPart)); + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + else + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg = m_contiguousNodes[nodeIndex].m_aabbMinOrg; + targetBvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg = m_contiguousNodes[nodeIndex].m_aabbMaxOrg; + + targetBvh->m_contiguousNodes[nodeIndex].m_escapeIndex = m_contiguousNodes[nodeIndex].m_escapeIndex; + targetBvh->m_contiguousNodes[nodeIndex].m_subPart = m_contiguousNodes[nodeIndex].m_subPart; + targetBvh->m_contiguousNodes[nodeIndex].m_triangleIndex = m_contiguousNodes[nodeIndex].m_triangleIndex; + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_contiguousNodes.initializeFromBuffer(NULL, 0, 0); + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + targetBvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, m_subtreeHeaderCount, m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_rootNodeIndex)); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(m_SubtreeHeaders[i].m_subtreeSize)); + } + } + else + { + for (int i = 0; i < m_subtreeHeaderCount; i++) + { + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = (m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = (m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = (m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = (m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = (m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + targetBvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = (m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + targetBvh->m_SubtreeHeaders[i].m_rootNodeIndex = (m_SubtreeHeaders[i].m_rootNodeIndex); + targetBvh->m_SubtreeHeaders[i].m_subtreeSize = (m_SubtreeHeaders[i].m_subtreeSize); + + // need to clear padding in destination buffer + targetBvh->m_SubtreeHeaders[i].m_padding[0] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[1] = 0; + targetBvh->m_SubtreeHeaders[i].m_padding[2] = 0; + } + } + nodeData += sizeof(btBvhSubtreeInfo) * m_subtreeHeaderCount; + + // this clears the pointer in the member variable it doesn't really do anything to the data + // it does call the destructor on the contained objects, but they are all classes with no destructor defined + // so the memory (which is not freed) is left alone + targetBvh->m_SubtreeHeaders.initializeFromBuffer(NULL, 0, 0); + + // this wipes the virtual function table pointer at the start of the buffer for the class + *((void**)o_alignedDataBuffer) = NULL; + + return true; +} + +btQuantizedBvh *btQuantizedBvh::deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian) +{ + + if (i_alignedDataBuffer == NULL)// || (((unsigned)i_alignedDataBuffer & BVH_ALIGNMENT_MASK) != 0)) + { + return NULL; + } + btQuantizedBvh *bvh = (btQuantizedBvh *)i_alignedDataBuffer; + + if (i_swapEndian) + { + bvh->m_curNodeIndex = static_cast(btSwapEndian(bvh->m_curNodeIndex)); + + btUnSwapVector3Endian(bvh->m_bvhAabbMin); + btUnSwapVector3Endian(bvh->m_bvhAabbMax); + btUnSwapVector3Endian(bvh->m_bvhQuantization); + + bvh->m_traversalMode = (btTraversalMode)btSwapEndian(bvh->m_traversalMode); + bvh->m_subtreeHeaderCount = static_cast(btSwapEndian(bvh->m_subtreeHeaderCount)); + } + + unsigned int calculatedBufSize = bvh->calculateSerializeBufferSize(); + btAssert(calculatedBufSize <= i_dataBufferSize); + + if (calculatedBufSize > i_dataBufferSize) + { + return NULL; + } + + unsigned char *nodeData = (unsigned char *)bvh; + nodeData += sizeof(btQuantizedBvh); + + unsigned sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + int nodeCount = bvh->m_curNodeIndex; + + // Must call placement new to fill in virtual function table, etc, but we don't want to overwrite most data, so call a special version of the constructor + // Also, m_leafNodes and m_quantizedLeafNodes will be initialized to default values by the constructor + new (bvh) btQuantizedBvh(*bvh, false); + + if (bvh->m_useQuantization) + { + bvh->m_quantizedContiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[1]); + bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[2]); + + bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = static_cast(btSwapEndian(bvh->m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex)); + } + } + nodeData += sizeof(btQuantizedBvhNode) * nodeCount; + } + else + { + bvh->m_contiguousNodes.initializeFromBuffer(nodeData, nodeCount, nodeCount); + + if (i_swapEndian) + { + for (int nodeIndex = 0; nodeIndex < nodeCount; nodeIndex++) + { + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMinOrg); + btUnSwapVector3Endian(bvh->m_contiguousNodes[nodeIndex].m_aabbMaxOrg); + + bvh->m_contiguousNodes[nodeIndex].m_escapeIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_escapeIndex)); + bvh->m_contiguousNodes[nodeIndex].m_subPart = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_subPart)); + bvh->m_contiguousNodes[nodeIndex].m_triangleIndex = static_cast(btSwapEndian(bvh->m_contiguousNodes[nodeIndex].m_triangleIndex)); + } + } + nodeData += sizeof(btOptimizedBvhNode) * nodeCount; + } + + sizeToAdd = 0;//(BVH_ALIGNMENT-((unsigned)nodeData & BVH_ALIGNMENT_MASK))&BVH_ALIGNMENT_MASK; + nodeData += sizeToAdd; + + // Now serialize the subtree headers + bvh->m_SubtreeHeaders.initializeFromBuffer(nodeData, bvh->m_subtreeHeaderCount, bvh->m_subtreeHeaderCount); + if (i_swapEndian) + { + for (int i = 0; i < bvh->m_subtreeHeaderCount; i++) + { + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMin[2]); + + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[0]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[1]); + bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2] = btSwapEndian(bvh->m_SubtreeHeaders[i].m_quantizedAabbMax[2]); + + bvh->m_SubtreeHeaders[i].m_rootNodeIndex = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_rootNodeIndex)); + bvh->m_SubtreeHeaders[i].m_subtreeSize = static_cast(btSwapEndian(bvh->m_SubtreeHeaders[i].m_subtreeSize)); + } + } + + return bvh; +} + +// Constructor that prevents btVector3's default constructor from being called +btQuantizedBvh::btQuantizedBvh(btQuantizedBvh &self, bool /* ownsMemory */) : +m_bvhAabbMin(self.m_bvhAabbMin), +m_bvhAabbMax(self.m_bvhAabbMax), +m_bvhQuantization(self.m_bvhQuantization), +m_bulletVersion(BT_BULLET_VERSION) +{ + +} + + + + diff --git a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index bc4585dac..ced457b60 100644 --- a/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -1,473 +1,473 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef QUANTIZED_BVH_H -#define QUANTIZED_BVH_H - -//#define DEBUG_CHECK_DEQUANTIZATION 1 -#ifdef DEBUG_CHECK_DEQUANTIZATION -#ifdef __SPU__ -#define printf spu_printf -#endif //__SPU__ - -#include -#include -#endif //DEBUG_CHECK_DEQUANTIZATION - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedAllocator.h" - - -//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp - - -//Note: currently we have 16 bytes per quantized node -#define MAX_SUBTREE_SIZE_IN_BYTES 2048 - -// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one -// actually) triangles each (since the sign bit is reserved -#define MAX_NUM_PARTS_IN_BITS 10 - -///btQuantizedBvhNode is a compressed aabb node, 16 bytes. -///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). -ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes - int m_escapeIndexOrTriangleIndex; - - bool isLeafNode() const - { - //skipindex is negative (internal node), triangleindex >=0 (leafnode) - return (m_escapeIndexOrTriangleIndex >= 0); - } - int getEscapeIndex() const - { - btAssert(!isLeafNode()); - return -m_escapeIndexOrTriangleIndex; - } - int getTriangleIndex() const - { - btAssert(isLeafNode()); - // Get only the lower bits where the triangle index is stored - return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); - } - int getPartId() const - { - btAssert(isLeafNode()); - // Get only the highest bits where the part index is stored - return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); - } -} -; - -/// btOptimizedBvhNode contains both internal and leaf node information. -/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. -ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //32 bytes - btVector3 m_aabbMinOrg; - btVector3 m_aabbMaxOrg; - - //4 - int m_escapeIndex; - - //8 - //for child nodes - int m_subPart; - int m_triangleIndex; - int m_padding[5];//bad, due to alignment - - -}; - - -///btBvhSubtreeInfo provides info to gather a subtree of limited size -ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo -{ -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - //12 bytes - unsigned short int m_quantizedAabbMin[3]; - unsigned short int m_quantizedAabbMax[3]; - //4 bytes, points to the root of the subtree - int m_rootNodeIndex; - //4 bytes - int m_subtreeSize; - int m_padding[3]; - - btBvhSubtreeInfo() - { - //memset(&m_padding[0], 0, sizeof(m_padding)); - } - - - void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) - { - m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; - m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; - m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; - m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; - m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; - m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; - } -} -; - - -class btNodeOverlapCallback -{ -public: - virtual ~btNodeOverlapCallback() {}; - - virtual void processNode(int subPart, int triangleIndex) = 0; -}; - -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btAlignedObjectArray.h" - - - -///for code readability: -typedef btAlignedObjectArray NodeArray; -typedef btAlignedObjectArray QuantizedNodeArray; -typedef btAlignedObjectArray BvhSubtreeInfoArray; - - -///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. -///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. -///It is recommended to use quantization for better performance and lower memory requirements. -ATTRIBUTE_ALIGNED16(class) btQuantizedBvh -{ -public: - enum btTraversalMode - { - TRAVERSAL_STACKLESS = 0, - TRAVERSAL_STACKLESS_CACHE_FRIENDLY, - TRAVERSAL_RECURSIVE - }; - -protected: - - - btVector3 m_bvhAabbMin; - btVector3 m_bvhAabbMax; - btVector3 m_bvhQuantization; - - int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. - - int m_curNodeIndex; - //quantization data - bool m_useQuantization; - - - - NodeArray m_leafNodes; - NodeArray m_contiguousNodes; - QuantizedNodeArray m_quantizedLeafNodes; - QuantizedNodeArray m_quantizedContiguousNodes; - - btTraversalMode m_traversalMode; - BvhSubtreeInfoArray m_SubtreeHeaders; - - //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray - int m_subtreeHeaderCount; - - - - - - ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) - ///this might be refactored into a virtual, it is usually not calculated at run-time - void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) - { - if (m_useQuantization) - { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); - } else - { - m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; - - } - } - void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) - { - if (m_useQuantization) - { - quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); - } else - { - m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; - } - } - - btVector3 getAabbMin(int nodeIndex) const - { - if (m_useQuantization) - { - return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); - } - //non-quantized - return m_leafNodes[nodeIndex].m_aabbMinOrg; - - } - btVector3 getAabbMax(int nodeIndex) const - { - if (m_useQuantization) - { - return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); - } - //non-quantized - return m_leafNodes[nodeIndex].m_aabbMaxOrg; - - } - - - void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) - { - if (m_useQuantization) - { - m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; - } - else - { - m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; - } - - } - - void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) - { - if (m_useQuantization) - { - unsigned short int quantizedAabbMin[3]; - unsigned short int quantizedAabbMax[3]; - quantize(quantizedAabbMin,newAabbMin,0); - quantize(quantizedAabbMax,newAabbMax,1); - for (int i=0;i<3;i++) - { - if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) - m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; - - if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) - m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; - - } - } else - { - //non-quantized - m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); - m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); - } - } - - void swapLeafNodes(int firstIndex,int secondIndex); - - void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); - -protected: - - - - void buildTree (int startIndex,int endIndex); - - int calcSplittingAxis(int startIndex,int endIndex); - - int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); - - void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; - void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; - void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; - - ///tree traversal designed for small-memory processors like PS3 SPU - void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; - - ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal - void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; - - ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal - void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; - - - - - void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btQuantizedBvh(); - - virtual ~btQuantizedBvh(); - - - ///***************************************** expert/internal use only ************************* - void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); - QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } - ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized - void buildInternal(); - ///***************************************** expert/internal use only ************************* - - void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; - void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; - - SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const - { - - btAssert(m_useQuantization); - - btAssert(point.getX() <= m_bvhAabbMax.getX()); - btAssert(point.getY() <= m_bvhAabbMax.getY()); - btAssert(point.getZ() <= m_bvhAabbMax.getZ()); - - btAssert(point.getX() >= m_bvhAabbMin.getX()); - btAssert(point.getY() >= m_bvhAabbMin.getY()); - btAssert(point.getZ() >= m_bvhAabbMin.getZ()); - - btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; - ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative - ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) - ///@todo: double-check this - if (isMax) - { - out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); - out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1)); - out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1)); - } else - { - out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); - out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); - out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); - } - - -#ifdef DEBUG_CHECK_DEQUANTIZATION - btVector3 newPoint = unQuantize(out); - if (isMax) - { - if (newPoint.getX() < point.getX()) - { - printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); - } - if (newPoint.getY() < point.getY()) - { - printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); - } - if (newPoint.getZ() < point.getZ()) - { - - printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); - } - } else - { - if (newPoint.getX() > point.getX()) - { - printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); - } - if (newPoint.getY() > point.getY()) - { - printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); - } - if (newPoint.getZ() > point.getZ()) - { - printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); - } - } -#endif //DEBUG_CHECK_DEQUANTIZATION - - } - - - SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const - { - - btAssert(m_useQuantization); - - btVector3 clampedPoint(point2); - clampedPoint.setMax(m_bvhAabbMin); - clampedPoint.setMin(m_bvhAabbMax); - - quantize(out,clampedPoint,isMax); - - } - - SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const - { - btVector3 vecOut; - vecOut.setValue( - (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), - (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), - (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); - vecOut += m_bvhAabbMin; - return vecOut; - } - - ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. - void setTraversalMode(btTraversalMode traversalMode) - { - m_traversalMode = traversalMode; - } - - - SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() - { - return m_quantizedContiguousNodes; - } - - - SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() - { - return m_SubtreeHeaders; - } - - - /////Calculate space needed to store BVH for serialization - unsigned calculateSerializeBufferSize(); - - /// Data buffer MUST be 16 byte aligned - virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); - - ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' - static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); - - static unsigned int getAlignmentSerializationPadding(); - - SIMD_FORCE_INLINE bool isQuantized() - { - return m_useQuantization; - } - -private: - // Special "copy" constructor that allows for in-place deserialization - // Prevents btVector3's default constructor from being called, but doesn't inialize much else - // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need) - btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); - -} -; - - -#endif //QUANTIZED_BVH_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef QUANTIZED_BVH_H +#define QUANTIZED_BVH_H + +//#define DEBUG_CHECK_DEQUANTIZATION 1 +#ifdef DEBUG_CHECK_DEQUANTIZATION +#ifdef __SPU__ +#define printf spu_printf +#endif //__SPU__ + +#include +#include +#endif //DEBUG_CHECK_DEQUANTIZATION + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedAllocator.h" + + +//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp + + +//Note: currently we have 16 bytes per quantized node +#define MAX_SUBTREE_SIZE_IN_BYTES 2048 + +// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one +// actually) triangles each (since the sign bit is reserved +#define MAX_NUM_PARTS_IN_BITS 10 + +///btQuantizedBvhNode is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; + + bool isLeafNode() const + { + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (m_escapeIndexOrTriangleIndex >= 0); + } + int getEscapeIndex() const + { + btAssert(!isLeafNode()); + return -m_escapeIndexOrTriangleIndex; + } + int getTriangleIndex() const + { + btAssert(isLeafNode()); + // Get only the lower bits where the triangle index is stored + return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); + } + int getPartId() const + { + btAssert(isLeafNode()); + // Get only the highest bits where the part index is stored + return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + } +} +; + +/// btOptimizedBvhNode contains both internal and leaf node information. +/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. +ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //32 bytes + btVector3 m_aabbMinOrg; + btVector3 m_aabbMaxOrg; + + //4 + int m_escapeIndex; + + //8 + //for child nodes + int m_subPart; + int m_triangleIndex; + int m_padding[5];//bad, due to alignment + + +}; + + +///btBvhSubtreeInfo provides info to gather a subtree of limited size +ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo +{ +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; + + btBvhSubtreeInfo() + { + //memset(&m_padding[0], 0, sizeof(m_padding)); + } + + + void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) + { + m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; + m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; + m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; + m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; + m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; + m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; + } +} +; + + +class btNodeOverlapCallback +{ +public: + virtual ~btNodeOverlapCallback() {}; + + virtual void processNode(int subPart, int triangleIndex) = 0; +}; + +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btAlignedObjectArray.h" + + + +///for code readability: +typedef btAlignedObjectArray NodeArray; +typedef btAlignedObjectArray QuantizedNodeArray; +typedef btAlignedObjectArray BvhSubtreeInfoArray; + + +///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. +///It is used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase. +///It is recommended to use quantization for better performance and lower memory requirements. +ATTRIBUTE_ALIGNED16(class) btQuantizedBvh +{ +public: + enum btTraversalMode + { + TRAVERSAL_STACKLESS = 0, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE + }; + +protected: + + + btVector3 m_bvhAabbMin; + btVector3 m_bvhAabbMax; + btVector3 m_bvhQuantization; + + int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. + + int m_curNodeIndex; + //quantization data + bool m_useQuantization; + + + + NodeArray m_leafNodes; + NodeArray m_contiguousNodes; + QuantizedNodeArray m_quantizedLeafNodes; + QuantizedNodeArray m_quantizedContiguousNodes; + + btTraversalMode m_traversalMode; + BvhSubtreeInfoArray m_SubtreeHeaders; + + //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray + int m_subtreeHeaderCount; + + + + + + ///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + ///this might be refactored into a virtual, it is usually not calculated at run-time + void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; + + } + } + void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) + { + if (m_useQuantization) + { + quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); + } else + { + m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; + } + } + + btVector3 getAabbMin(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMinOrg; + + } + btVector3 getAabbMax(int nodeIndex) const + { + if (m_useQuantization) + { + return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); + } + //non-quantized + return m_leafNodes[nodeIndex].m_aabbMaxOrg; + + } + + + void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) + { + if (m_useQuantization) + { + m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; + } + else + { + m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; + } + + } + + void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) + { + if (m_useQuantization) + { + unsigned short int quantizedAabbMin[3]; + unsigned short int quantizedAabbMax[3]; + quantize(quantizedAabbMin,newAabbMin,0); + quantize(quantizedAabbMax,newAabbMax,1); + for (int i=0;i<3;i++) + { + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] = quantizedAabbMin[i]; + + if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i]) + m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; + + } + } else + { + //non-quantized + m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); + m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); + } + } + + void swapLeafNodes(int firstIndex,int secondIndex); + + void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); + +protected: + + + + void buildTree (int startIndex,int endIndex); + + int calcSplittingAxis(int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); + + void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; + void walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const; + + ///tree traversal designed for small-memory processors like PS3 SPU + void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantizedBvhNode* currentNode,btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; + + ///use the 16-byte stackless 'skipindex' node tree to do a recursive traversal + void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; + + + + + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btQuantizedBvh(); + + virtual ~btQuantizedBvh(); + + + ///***************************************** expert/internal use only ************************* + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); + QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } + ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized + void buildInternal(); + ///***************************************** expert/internal use only ************************* + + void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; + void reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const; + + SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const + { + + btAssert(m_useQuantization); + + btAssert(point.getX() <= m_bvhAabbMax.getX()); + btAssert(point.getY() <= m_bvhAabbMax.getY()); + btAssert(point.getZ() <= m_bvhAabbMax.getZ()); + + btAssert(point.getX() >= m_bvhAabbMin.getX()); + btAssert(point.getY() >= m_bvhAabbMin.getY()); + btAssert(point.getZ() >= m_bvhAabbMin.getZ()); + + btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; + ///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative + ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly) + ///@todo: double-check this + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.getY()+btScalar(1.)) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); + } + + +#ifdef DEBUG_CHECK_DEQUANTIZATION + btVector3 newPoint = unQuantize(out); + if (isMax) + { + if (newPoint.getX() < point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() < point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() < point.getZ()) + { + + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } else + { + if (newPoint.getX() > point.getX()) + { + printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), newPoint.getX(),point.getX()); + } + if (newPoint.getY() > point.getY()) + { + printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), newPoint.getY(),point.getY()); + } + if (newPoint.getZ() > point.getZ()) + { + printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); + } + } +#endif //DEBUG_CHECK_DEQUANTIZATION + + } + + + SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const + { + + btAssert(m_useQuantization); + + btVector3 clampedPoint(point2); + clampedPoint.setMax(m_bvhAabbMin); + clampedPoint.setMin(m_bvhAabbMax); + + quantize(out,clampedPoint,isMax); + + } + + SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const + { + btVector3 vecOut; + vecOut.setValue( + (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), + (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), + (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); + vecOut += m_bvhAabbMin; + return vecOut; + } + + ///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. + void setTraversalMode(btTraversalMode traversalMode) + { + m_traversalMode = traversalMode; + } + + + SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() + { + return m_quantizedContiguousNodes; + } + + + SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() + { + return m_SubtreeHeaders; + } + + + /////Calculate space needed to store BVH for serialization + unsigned calculateSerializeBufferSize(); + + /// Data buffer MUST be 16 byte aligned + virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); + + ///deSerializeInPlace loads and initializes a BVH from a buffer in memory 'in place' + static btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian); + + static unsigned int getAlignmentSerializationPadding(); + + SIMD_FORCE_INLINE bool isQuantized() + { + return m_useQuantization; + } + +private: + // Special "copy" constructor that allows for in-place deserialization + // Prevents btVector3's default constructor from being called, but doesn't inialize much else + // ownsMemory should most likely be false if deserializing, and if you are not, don't call this (it also changes the function signature, which we need) + btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory); + +} +; + + +#endif //QUANTIZED_BVH_H diff --git a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index ae6fe2019..9a749a037 100644 --- a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -1,209 +1,209 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btScalar.h" -#include "SphereTriangleDetector.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - - -SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) -:m_sphere(sphere), -m_triangle(triangle), -m_contactBreakingThreshold(contactBreakingThreshold) -{ - -} - -void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) -{ - - (void)debugDraw; - const btTransform& transformA = input.m_transformA; - const btTransform& transformB = input.m_transformB; - - btVector3 point,normal; - btScalar timeOfImpact = btScalar(1.); - btScalar depth = btScalar(0.); -// output.m_distance = btScalar(1e30); - //move sphere into triangle space - btTransform sphereInTr = transformB.inverseTimes(transformA); - - if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) - { - if (swapResults) - { - btVector3 normalOnB = transformB.getBasis()*normal; - btVector3 normalOnA = -normalOnB; - btVector3 pointOnA = transformB*point+normalOnB*depth; - output.addContactPoint(normalOnA,pointOnA,depth); - } else - { - output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); - } - } - -} - -#define MAX_OVERLAP btScalar(0.) - - - -// See also geometrictools.com -// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); - -btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { - btVector3 diff = p - from; - btVector3 v = to - from; - btScalar t = v.dot(diff); - - if (t > 0) { - btScalar dotVV = v.dot(v); - if (t < dotVV) { - t /= dotVV; - diff -= t*v; - } else { - t = 1; - diff -= v; - } - } else - t = 0; - - nearest = from + t*v; - return diff.dot(diff); -} - -bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { - btVector3 lp(p); - btVector3 lnormal(normal); - - return pointInTriangle(vertices, lnormal, &lp); -} - -///combined discrete/continuous sphere-triangle -bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) -{ - - const btVector3* vertices = &m_triangle->getVertexPtr(0); - const btVector3& c = sphereCenter; - btScalar r = m_sphere->getRadius(); - - btVector3 delta (0,0,0); - - btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); - btVector3 p1ToCentre = c - vertices[0]; - btScalar distanceFromPlane = p1ToCentre.dot(normal); - - if (distanceFromPlane < btScalar(0.)) - { - //triangle facing the other way - - distanceFromPlane *= btScalar(-1.); - normal *= btScalar(-1.); - } - - btScalar contactMargin = contactBreakingThreshold; - bool isInsideContactPlane = distanceFromPlane < r + contactMargin; - bool isInsideShellPlane = distanceFromPlane < r; - - btScalar deltaDotNormal = delta.dot(normal); - if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) - return false; - - // Check for contact / intersection - bool hasContact = false; - btVector3 contactPoint; - if (isInsideContactPlane) { - if (facecontains(c,vertices,normal)) { - // Inside the contact wedge - touches a point on the shell plane - hasContact = true; - contactPoint = c - normal*distanceFromPlane; - } else { - // Could be inside one of the contact capsules - btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); - btVector3 nearestOnEdge; - for (int i = 0; i < m_triangle->getNumEdges(); i++) { - - btVector3 pa; - btVector3 pb; - - m_triangle->getEdge(i,pa,pb); - - btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule - hasContact = true; - contactPoint = nearestOnEdge; - } - - } - } - } - - if (hasContact) { - btVector3 contactToCentre = c - contactPoint; - btScalar distanceSqr = contactToCentre.length2(); - if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { - btScalar distance = btSqrt(distanceSqr); - resultNormal = contactToCentre; - resultNormal.normalize(); - point = contactPoint; - depth = -(r-distance); - return true; - } - - if (delta.dot(contactToCentre) >= btScalar(0.0)) - return false; - - // Moving towards the contact point -> collision - point = contactPoint; - timeOfImpact = btScalar(0.0); - return true; - } - - return false; -} - - -bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) -{ - const btVector3* p1 = &vertices[0]; - const btVector3* p2 = &vertices[1]; - const btVector3* p3 = &vertices[2]; - - btVector3 edge1( *p2 - *p1 ); - btVector3 edge2( *p3 - *p2 ); - btVector3 edge3( *p1 - *p3 ); - - btVector3 p1_to_p( *p - *p1 ); - btVector3 p2_to_p( *p - *p2 ); - btVector3 p3_to_p( *p - *p3 ); - - btVector3 edge1_normal( edge1.cross(normal)); - btVector3 edge2_normal( edge2.cross(normal)); - btVector3 edge3_normal( edge3.cross(normal)); - - btScalar r1, r2, r3; - r1 = edge1_normal.dot( p1_to_p ); - r2 = edge2_normal.dot( p2_to_p ); - r3 = edge3_normal.dot( p3_to_p ); - if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || - ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) - return true; - return false; - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btScalar.h" +#include "SphereTriangleDetector.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + + +SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) +:m_sphere(sphere), +m_triangle(triangle), +m_contactBreakingThreshold(contactBreakingThreshold) +{ + +} + +void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) +{ + + (void)debugDraw; + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + btVector3 point,normal; + btScalar timeOfImpact = btScalar(1.); + btScalar depth = btScalar(0.); +// output.m_distance = btScalar(1e30); + //move sphere into triangle space + btTransform sphereInTr = transformB.inverseTimes(transformA); + + if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) + { + if (swapResults) + { + btVector3 normalOnB = transformB.getBasis()*normal; + btVector3 normalOnA = -normalOnB; + btVector3 pointOnA = transformB*point+normalOnB*depth; + output.addContactPoint(normalOnA,pointOnA,depth); + } else + { + output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); + } + } + +} + +#define MAX_OVERLAP btScalar(0.) + + + +// See also geometrictools.com +// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); + +btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { + btVector3 diff = p - from; + btVector3 v = to - from; + btScalar t = v.dot(diff); + + if (t > 0) { + btScalar dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + } else { + t = 1; + diff -= v; + } + } else + t = 0; + + nearest = from + t*v; + return diff.dot(diff); +} + +bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { + btVector3 lp(p); + btVector3 lnormal(normal); + + return pointInTriangle(vertices, lnormal, &lp); +} + +///combined discrete/continuous sphere-triangle +bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) +{ + + const btVector3* vertices = &m_triangle->getVertexPtr(0); + const btVector3& c = sphereCenter; + btScalar r = m_sphere->getRadius(); + + btVector3 delta (0,0,0); + + btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); + normal.normalize(); + btVector3 p1ToCentre = c - vertices[0]; + btScalar distanceFromPlane = p1ToCentre.dot(normal); + + if (distanceFromPlane < btScalar(0.)) + { + //triangle facing the other way + + distanceFromPlane *= btScalar(-1.); + normal *= btScalar(-1.); + } + + btScalar contactMargin = contactBreakingThreshold; + bool isInsideContactPlane = distanceFromPlane < r + contactMargin; + bool isInsideShellPlane = distanceFromPlane < r; + + btScalar deltaDotNormal = delta.dot(normal); + if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) + return false; + + // Check for contact / intersection + bool hasContact = false; + btVector3 contactPoint; + if (isInsideContactPlane) { + if (facecontains(c,vertices,normal)) { + // Inside the contact wedge - touches a point on the shell plane + hasContact = true; + contactPoint = c - normal*distanceFromPlane; + } else { + // Could be inside one of the contact capsules + btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); + btVector3 nearestOnEdge; + for (int i = 0; i < m_triangle->getNumEdges(); i++) { + + btVector3 pa; + btVector3 pb; + + m_triangle->getEdge(i,pa,pb); + + btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); + if (distanceSqr < contactCapsuleRadiusSqr) { + // Yep, we're inside a capsule + hasContact = true; + contactPoint = nearestOnEdge; + } + + } + } + } + + if (hasContact) { + btVector3 contactToCentre = c - contactPoint; + btScalar distanceSqr = contactToCentre.length2(); + if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { + btScalar distance = btSqrt(distanceSqr); + resultNormal = contactToCentre; + resultNormal.normalize(); + point = contactPoint; + depth = -(r-distance); + return true; + } + + if (delta.dot(contactToCentre) >= btScalar(0.0)) + return false; + + // Moving towards the contact point -> collision + point = contactPoint; + timeOfImpact = btScalar(0.0); + return true; + } + + return false; +} + + +bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) +{ + const btVector3* p1 = &vertices[0]; + const btVector3* p2 = &vertices[1]; + const btVector3* p3 = &vertices[2]; + + btVector3 edge1( *p2 - *p1 ); + btVector3 edge2( *p3 - *p2 ); + btVector3 edge3( *p1 - *p3 ); + + btVector3 p1_to_p( *p - *p1 ); + btVector3 p2_to_p( *p - *p2 ); + btVector3 p3_to_p( *p - *p3 ); + + btVector3 edge1_normal( edge1.cross(normal)); + btVector3 edge2_normal( edge2.cross(normal)); + btVector3 edge3_normal( edge3.cross(normal)); + + btScalar r1, r2, r3; + r1 = edge1_normal.dot( p1_to_p ); + r2 = edge2_normal.dot( p2_to_p ); + r3 = edge3_normal.dot( p3_to_p ); + if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || + ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) + return true; + return false; + +} diff --git a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp index 8fef85ef6..7e5da6c58 100644 --- a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp @@ -1,47 +1,47 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btActivatingCollisionAlgorithm.h" -#include "btCollisionDispatcher.h" -#include "btCollisionObject.h" - -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) -:btCollisionAlgorithm(ci) -//, -//m_colObj0(0), -//m_colObj1(0) -{ -} -btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1) -:btCollisionAlgorithm(ci) -//, -//m_colObj0(0), -//m_colObj1(0) -{ -// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) -// { -// m_colObj0 = colObj0; -// m_colObj1 = colObj1; -// -// m_colObj0->activate(); -// m_colObj1->activate(); -// } -} - -btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() -{ -// m_colObj0->activate(); -// m_colObj1->activate(); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btActivatingCollisionAlgorithm.h" +#include "btCollisionDispatcher.h" +#include "btCollisionObject.h" + +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +} +btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1) +:btCollisionAlgorithm(ci) +//, +//m_colObj0(0), +//m_colObj1(0) +{ +// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) +// { +// m_colObj0 = colObj0; +// m_colObj1 = colObj1; +// +// m_colObj0->activate(); +// m_colObj1->activate(); +// } +} + +btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() +{ +// m_colObj0->activate(); +// m_colObj1->activate(); +} diff --git a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h index a1f8746c1..25fe08894 100644 --- a/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h @@ -1,36 +1,36 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H -#define __BT_ACTIVATING_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -///This class is not enabled yet (work-in-progress) to more aggressively activate objects. -class btActivatingCollisionAlgorithm : public btCollisionAlgorithm -{ -// btCollisionObject* m_colObj0; -// btCollisionObject* m_colObj1; - -public: - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); - - btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1); - - virtual ~btActivatingCollisionAlgorithm(); - -}; -#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H +#define __BT_ACTIVATING_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +///This class is not enabled yet (work-in-progress) to more aggressively activate objects. +class btActivatingCollisionAlgorithm : public btCollisionAlgorithm +{ +// btCollisionObject* m_colObj0; +// btCollisionObject* m_colObj1; + +public: + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); + + btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1); + + virtual ~btActivatingCollisionAlgorithm(); + +}; +#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp index 978e9e7ea..d3342c547 100644 --- a/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp @@ -1,85 +1,85 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btBoxBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btBoxBoxDetector.h" - -#define USE_PERSISTENT_CONTACTS 1 - -btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) -: btActivatingCollisionAlgorithm(ci,obj0,obj1), -m_ownManifold(false), -m_manifoldPtr(mf) -{ - if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); - m_ownManifold = true; - } -} - -btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - if (!m_manifoldPtr) - return; - - btCollisionObject* col0 = body0; - btCollisionObject* col1 = body1; - btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); - btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); - - - - /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->setPersistentManifold(m_manifoldPtr); -#ifndef USE_PERSISTENT_CONTACTS - m_manifoldPtr->clearManifold(); -#endif //USE_PERSISTENT_CONTACTS - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f; - input.m_transformA = body0->getWorldTransform(); - input.m_transformB = body1->getWorldTransform(); - - btBoxBoxDetector detector(box0,box1); - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); - -#ifdef USE_PERSISTENT_CONTACTS - // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added - if (m_ownManifold) - { - resultOut->refreshContactPoints(); - } -#endif //USE_PERSISTENT_CONTACTS - -} - -btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - //not yet - return 1.f; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btBoxBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btBoxBoxDetector.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) +: btActivatingCollisionAlgorithm(ci,obj0,obj1), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); + m_ownManifold = true; + } +} + +btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* col0 = body0; + btCollisionObject* col1 = body1; + btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); + btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); + + + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); +#ifndef USE_PERSISTENT_CONTACTS + m_manifoldPtr->clearManifold(); +#endif //USE_PERSISTENT_CONTACTS + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + btBoxBoxDetector detector(box0,box1); + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +#ifdef USE_PERSISTENT_CONTACTS + // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } +#endif //USE_PERSISTENT_CONTACTS + +} + +btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index 1247c6544..e7d2cc25c 100644 --- a/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -1,66 +1,66 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BOX_BOX__COLLISION_ALGORITHM_H -#define BOX_BOX__COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; - -///box-box collision detection -class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - -public: - btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btActivatingCollisionAlgorithm(ci) {} - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); - - virtual ~btBoxBoxCollisionAlgorithm(); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - int bbsize = sizeof(btBoxBoxCollisionAlgorithm); - void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1); - } - }; - -}; - -#endif //BOX_BOX__COLLISION_ALGORITHM_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BOX_BOX__COLLISION_ALGORITHM_H +#define BOX_BOX__COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; + +///box-box collision detection +class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btBoxBoxCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btBoxBoxCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //BOX_BOX__COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp index 83dcb5c6f..31353f1b2 100644 --- a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp +++ b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp @@ -1,689 +1,689 @@ - -/* - * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - Bullet Continuous Collision Detection and Physics Library - Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///ODE box-box collision detection is adapted to work with Bullet - -#include "btBoxBoxDetector.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" - -#include -#include - -btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2) -: m_box1(box1), -m_box2(box2) -{ - -} - - -// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and -// generate contact points. this returns 0 if there is no contact otherwise -// it returns the number of contacts generated. -// `normal' returns the contact normal. -// `depth' returns the maximum penetration depth along that normal. -// `return_code' returns a number indicating the type of contact that was -// detected: -// 1,2,3 = box 2 intersects with a face of box 1 -// 4,5,6 = box 1 intersects with a face of box 2 -// 7..15 = edge-edge contact -// `maxc' is the maximum number of contacts allowed to be generated, i.e. -// the size of the `contact' array. -// `contact' and `skip' are the contact array information provided to the -// collision functions. this function only fills in the position and depth -// fields. -struct dContactGeom; -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -#define dInfinity FLT_MAX - - -/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } -PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } -PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } -*/ -static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } -static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } -static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } -static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } -#define dMULTIPLYOP1_331(A,op,B,C) \ -{\ - (A)[0] op dDOT41((B),(C)); \ - (A)[1] op dDOT41((B+1),(C)); \ - (A)[2] op dDOT41((B+2),(C)); \ -} - -#define dMULTIPLYOP0_331(A,op,B,C) \ -{ \ - (A)[0] op dDOT((B),(C)); \ - (A)[1] op dDOT((B+4),(C)); \ - (A)[2] op dDOT((B+8),(C)); \ -} - -#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) - -typedef btScalar dMatrix3[4*3]; - -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta); -void dLineClosestApproach (const btVector3& pa, const btVector3& ua, - const btVector3& pb, const btVector3& ub, - btScalar *alpha, btScalar *beta) -{ - btVector3 p; - p[0] = pb[0] - pa[0]; - p[1] = pb[1] - pa[1]; - p[2] = pb[2] - pa[2]; - btScalar uaub = dDOT(ua,ub); - btScalar q1 = dDOT(ua,p); - btScalar q2 = -dDOT(ub,p); - btScalar d = 1-uaub*uaub; - if (d <= btScalar(0.0001f)) { - // @@@ this needs to be made more robust - *alpha = 0; - *beta = 0; - } - else { - d = 1.f/d; - *alpha = (q1 + uaub*q2)*d; - *beta = (uaub*q1 + q2)*d; - } -} - - - -// find all the intersection points between the 2D rectangle with vertices -// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), -// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). -// -// the intersection points are returned as x,y pairs in the 'ret' array. -// the number of intersection points is returned by the function (this will -// be in the range 0 to 8). - -static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) -{ - // q (and r) contain nq (and nr) coordinate points for the current (and - // chopped) polygons - int nq=4,nr=0; - btScalar buffer[16]; - btScalar *q = p; - btScalar *r = ret; - for (int dir=0; dir <= 1; dir++) { - // direction notation: xy[0] = x axis, xy[1] = y axis - for (int sign=-1; sign <= 1; sign += 2) { - // chop q along the line xy[dir] = sign*h[dir] - btScalar *pq = q; - btScalar *pr = r; - nr = 0; - for (int i=nq; i > 0; i--) { - // go through all points in q and all lines between adjacent points - if (sign*pq[dir] < h[dir]) { - // this point is inside the chopping line - pr[0] = pq[0]; - pr[1] = pq[1]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - btScalar *nextq = (i > 1) ? pq+2 : q; - if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { - // this line crosses the chopping line - pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / - (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); - pr[dir] = sign*h[dir]; - pr += 2; - nr++; - if (nr & 8) { - q = r; - goto done; - } - } - pq += 2; - } - q = r; - r = (q==ret) ? buffer : ret; - nq = nr; - } - } - done: - if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); - return nr; -} - - -#define M__PI 3.14159265f - -// given n points in the plane (array p, of size 2*n), generate m points that -// best represent the whole set. the definition of 'best' here is not -// predetermined - the idea is to select points that give good box-box -// collision detection behavior. the chosen point indexes are returned in the -// array iret (of size m). 'i0' is always the first entry in the array. -// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be -// in the range [0..n-1]. - -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); -void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) -{ - // compute the centroid of the polygon in cx,cy - int i,j; - btScalar a,cx,cy,q; - if (n==1) { - cx = p[0]; - cy = p[1]; - } - else if (n==2) { - cx = btScalar(0.5)*(p[0] + p[2]); - cy = btScalar(0.5)*(p[1] + p[3]); - } - else { - a = 0; - cx = 0; - cy = 0; - for (i=0; i<(n-1); i++) { - q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; - a += q; - cx += q*(p[i*2]+p[i*2+2]); - cy += q*(p[i*2+1]+p[i*2+3]); - } - q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; - if (btFabs(a+q) > SIMD_EPSILON) - { - a = 1.f/(btScalar(3.0)*(a+q)); - } else - { - a=1e30f; - } - cx = a*(cx + q*(p[n*2-2]+p[0])); - cy = a*(cy + q*(p[n*2-1]+p[1])); - } - - // compute the angle of each point w.r.t. the centroid - btScalar A[8]; - for (i=0; i M__PI) a -= 2*M__PI; - btScalar maxdiff=1e9,diff; - - *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 - - for (i=0; i M__PI) diff = 2*M__PI - diff; - if (diff < maxdiff) { - maxdiff = diff; - *iret = i; - } - } - } -#if defined(DEBUG) || defined (_DEBUG) - btAssert (*iret != i0); // ensure iret got set -#endif - avail[*iret] = 0; - iret++; - } -} - - - -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); -int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, - const btVector3& side1, const btVector3& p2, - const dMatrix3 R2, const btVector3& side2, - btVector3& normal, btScalar *depth, int *return_code, - int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) -{ - const btScalar fudge_factor = btScalar(1.05); - btVector3 p,pp,normalC; - const btScalar *normalR = 0; - btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, - Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; - int i,j,invert_normal,code; - - // get vector from centers of box 1 to box 2, relative to box 1 - p = p2 - p1; - dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 - - // get side lengths / 2 - A[0] = side1[0]*btScalar(0.5); - A[1] = side1[1]*btScalar(0.5); - A[2] = side1[2]*btScalar(0.5); - B[0] = side2[0]*btScalar(0.5); - B[1] = side2[1]*btScalar(0.5); - B[2] = side2[2]*btScalar(0.5); - - // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 - R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); - R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); - R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); - - Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); - Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); - Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); - - // for all 15 possible separating axes: - // * see if the axis separates the boxes. if so, return 0. - // * find the depth of the penetration along the separating axis (s2) - // * if this is the largest depth so far, record it. - // the normal vector will be set to the separating axis with the smallest - // depth. note: normalR is set to point to a column of R1 or R2 if that is - // the smallest depth normal so far. otherwise normalR is 0 and normalC is - // set to a vector relative to body 1. invert_normal is 1 if the sign of - // the normal should be flipped. - -#define TST(expr1,expr2,norm,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - if (s2 > s) { \ - s = s2; \ - normalR = norm; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } - - s = -dInfinity; - invert_normal = 0; - code = 0; - - // separating axis = u1,u2,u3 - TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); - TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); - TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); - - // separating axis = v1,v2,v3 - TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); - TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); - TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); - - // note: cross product axes need to be scaled when s is computed. - // normal (n1,n2,n3) is relative to box 1. -#undef TST -#define TST(expr1,expr2,n1,n2,n3,cc) \ - s2 = btFabs(expr1) - (expr2); \ - if (s2 > 0) return 0; \ - l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ - if (l > 0) { \ - s2 /= l; \ - if (s2*fudge_factor > s) { \ - s = s2; \ - normalR = 0; \ - normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ - invert_normal = ((expr1) < 0); \ - code = (cc); \ - } \ - } - - // separating axis = u1 x (v1,v2,v3) - TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); - TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); - TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); - - // separating axis = u2 x (v1,v2,v3) - TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); - TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); - TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); - - // separating axis = u3 x (v1,v2,v3) - TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); - TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); - TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); - -#undef TST - - if (!code) return 0; - - // if we get to this point, the boxes interpenetrate. compute the normal - // in global coordinates. - if (normalR) { - normal[0] = normalR[0]; - normal[1] = normalR[4]; - normal[2] = normalR[8]; - } - else { - dMULTIPLY0_331 (normal,R1,normalC); - } - if (invert_normal) { - normal[0] = -normal[0]; - normal[1] = -normal[1]; - normal[2] = -normal[2]; - } - *depth = -s; - - // compute contact point(s) - - if (code > 6) { - // an edge from box 1 touches an edge from box 2. - // find a point pa on the intersecting edge of box 1 - btVector3 pa; - btScalar sign; - for (i=0; i<3; i++) pa[i] = p1[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); - for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; - } - - // find a point pb on the intersecting edge of box 2 - btVector3 pb; - for (i=0; i<3; i++) pb[i] = p2[i]; - for (j=0; j<3; j++) { - sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); - for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; - } - - btScalar alpha,beta; - btVector3 ua,ub; - for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; - for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; - - dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); - for (i=0; i<3; i++) pa[i] += ua[i]*alpha; - for (i=0; i<3; i++) pb[i] += ub[i]*beta; - - { - - //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); - //contact[0].depth = *depth; - btVector3 pointInWorld; - -#ifdef USE_CENTER_POINT - for (i=0; i<3; i++) - pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); - output.addContactPoint(-normal,pointInWorld,-*depth); -#else - output.addContactPoint(-normal,pb,-*depth); -#endif // - *return_code = code; - } - return 1; - } - - // okay, we have a face-something intersection (because the separating - // axis is perpendicular to a face). define face 'a' to be the reference - // face (i.e. the normal vector is perpendicular to this) and face 'b' to be - // the incident face (the closest face of the other box). - - const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; - if (code <= 3) { - Ra = R1; - Rb = R2; - pa = p1; - pb = p2; - Sa = A; - Sb = B; - } - else { - Ra = R2; - Rb = R1; - pa = p2; - pb = p1; - Sa = B; - Sb = A; - } - - // nr = normal vector of reference face dotted with axes of incident box. - // anr = absolute values of nr. - btVector3 normal2,nr,anr; - if (code <= 3) { - normal2[0] = normal[0]; - normal2[1] = normal[1]; - normal2[2] = normal[2]; - } - else { - normal2[0] = -normal[0]; - normal2[1] = -normal[1]; - normal2[2] = -normal[2]; - } - dMULTIPLY1_331 (nr,Rb,normal2); - anr[0] = btFabs (nr[0]); - anr[1] = btFabs (nr[1]); - anr[2] = btFabs (nr[2]); - - // find the largest compontent of anr: this corresponds to the normal - // for the indident face. the other axis numbers of the indicent face - // are stored in a1,a2. - int lanr,a1,a2; - if (anr[1] > anr[0]) { - if (anr[1] > anr[2]) { - a1 = 0; - lanr = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - else { - if (anr[0] > anr[2]) { - lanr = 0; - a1 = 1; - a2 = 2; - } - else { - a1 = 0; - a2 = 1; - lanr = 2; - } - } - - // compute center point of incident face, in reference-face coordinates - btVector3 center; - if (nr[lanr] < 0) { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; - } - else { - for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; - } - - // find the normal and non-normal axis numbers of the reference box - int codeN,code1,code2; - if (code <= 3) codeN = code-1; else codeN = code-4; - if (codeN==0) { - code1 = 1; - code2 = 2; - } - else if (codeN==1) { - code1 = 0; - code2 = 2; - } - else { - code1 = 0; - code2 = 1; - } - - // find the four corners of the incident face, in reference-face coordinates - btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) - btScalar c1,c2,m11,m12,m21,m22; - c1 = dDOT14 (center,Ra+code1); - c2 = dDOT14 (center,Ra+code2); - // optimize this? - we have already computed this data above, but it is not - // stored in an easy-to-index format. for now it's quicker just to recompute - // the four dot products. - m11 = dDOT44 (Ra+code1,Rb+a1); - m12 = dDOT44 (Ra+code1,Rb+a2); - m21 = dDOT44 (Ra+code2,Rb+a1); - m22 = dDOT44 (Ra+code2,Rb+a2); - { - btScalar k1 = m11*Sb[a1]; - btScalar k2 = m21*Sb[a1]; - btScalar k3 = m12*Sb[a2]; - btScalar k4 = m22*Sb[a2]; - quad[0] = c1 - k1 - k3; - quad[1] = c2 - k2 - k4; - quad[2] = c1 - k1 + k3; - quad[3] = c2 - k2 + k4; - quad[4] = c1 + k1 + k3; - quad[5] = c2 + k2 + k4; - quad[6] = c1 + k1 - k3; - quad[7] = c2 + k2 - k4; - } - - // find the size of the reference face - btScalar rect[2]; - rect[0] = Sa[code1]; - rect[1] = Sa[code2]; - - // intersect the incident and reference faces - btScalar ret[16]; - int n = intersectRectQuad2 (rect,quad,ret); - if (n < 1) return 0; // this should never happen - - // convert the intersection points into reference-face coordinates, - // and compute the contact position and depth for each point. only keep - // those points that have a positive (penetrating) depth. delete points in - // the 'ret' array as necessary so that 'point' and 'ret' correspond. - btScalar point[3*8]; // penetrating contact points - btScalar dep[8]; // depths for those points - btScalar det1 = 1.f/(m11*m22 - m12*m21); - m11 *= det1; - m12 *= det1; - m21 *= det1; - m22 *= det1; - int cnum = 0; // number of penetrating contact points found - for (j=0; j < n; j++) { - btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); - btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); - for (i=0; i<3; i++) point[cnum*3+i] = - center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; - dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); - if (dep[cnum] >= 0) { - ret[cnum*2] = ret[j*2]; - ret[cnum*2+1] = ret[j*2+1]; - cnum++; - } - } - if (cnum < 1) return 0; // this should never happen - - // we can't generate more contacts than we actually have - if (maxc > cnum) maxc = cnum; - if (maxc < 1) maxc = 1; - - if (cnum <= maxc) { - // we have less contacts than we need, so we use them all - for (j=0; j < cnum; j++) { - - //AddContactPoint... - - //dContactGeom *con = CONTACT(contact,skip*j); - //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; - //con->depth = dep[j]; - - btVector3 pointInWorld; - for (i=0; i<3; i++) - pointInWorld[i] = point[j*3+i] + pa[i]; - output.addContactPoint(-normal,pointInWorld,-dep[j]); - - } - } - else { - // we have more contacts than are wanted, some of them must be culled. - // find the deepest point, it is always the first contact. - int i1 = 0; - btScalar maxdepth = dep[0]; - for (i=1; i maxdepth) { - maxdepth = dep[i]; - i1 = i; - } - } - - int iret[8]; - cullPoints2 (cnum,ret,maxc,i1,iret); - - for (j=0; j < maxc; j++) { -// dContactGeom *con = CONTACT(contact,skip*j); - // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; - // con->depth = dep[iret[j]]; - - btVector3 posInWorld; - for (i=0; i<3; i++) - posInWorld[i] = point[iret[j]*3+i] + pa[i]; - output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); - } - cnum = maxc; - } - - *return_code = code; - return cnum; -} - -void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) -{ - - const btTransform& transformA = input.m_transformA; - const btTransform& transformB = input.m_transformB; - - int skip = 0; - dContactGeom *contact = 0; - - dMatrix3 R1; - dMatrix3 R2; - - for (int j=0;j<3;j++) - { - R1[0+4*j] = transformA.getBasis()[j].x(); - R2[0+4*j] = transformB.getBasis()[j].x(); - - R1[1+4*j] = transformA.getBasis()[j].y(); - R2[1+4*j] = transformB.getBasis()[j].y(); - - - R1[2+4*j] = transformA.getBasis()[j].z(); - R2[2+4*j] = transformB.getBasis()[j].z(); - - } - - - - btVector3 normal; - btScalar depth; - int return_code; - int maxc = 4; - - - dBoxBox2 (transformA.getOrigin(), - R1, - 2.f*m_box1->getHalfExtentsWithMargin(), - transformB.getOrigin(), - R2, - 2.f*m_box2->getHalfExtentsWithMargin(), - normal, &depth, &return_code, - maxc, contact, skip, - output - ); - -} + +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///ODE box-box collision detection is adapted to work with Bullet + +#include "btBoxBoxDetector.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +#include +#include + +btBoxBoxDetector::btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2) +: m_box1(box1), +m_box2(box2) +{ + +} + + +// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and +// generate contact points. this returns 0 if there is no contact otherwise +// it returns the number of contacts generated. +// `normal' returns the contact normal. +// `depth' returns the maximum penetration depth along that normal. +// `return_code' returns a number indicating the type of contact that was +// detected: +// 1,2,3 = box 2 intersects with a face of box 1 +// 4,5,6 = box 1 intersects with a face of box 2 +// 7..15 = edge-edge contact +// `maxc' is the maximum number of contacts allowed to be generated, i.e. +// the size of the `contact' array. +// `contact' and `skip' are the contact array information provided to the +// collision functions. this function only fills in the position and depth +// fields. +struct dContactGeom; +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +#define dInfinity FLT_MAX + + +/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); } +PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); } +PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); } +*/ +static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); } +static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); } +static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); } +static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); } +#define dMULTIPLYOP1_331(A,op,B,C) \ +{\ + (A)[0] op dDOT41((B),(C)); \ + (A)[1] op dDOT41((B+1),(C)); \ + (A)[2] op dDOT41((B+2),(C)); \ +} + +#define dMULTIPLYOP0_331(A,op,B,C) \ +{ \ + (A)[0] op dDOT((B),(C)); \ + (A)[1] op dDOT((B+4),(C)); \ + (A)[2] op dDOT((B+8),(C)); \ +} + +#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C) +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) + +typedef btScalar dMatrix3[4*3]; + +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta); +void dLineClosestApproach (const btVector3& pa, const btVector3& ua, + const btVector3& pb, const btVector3& ub, + btScalar *alpha, btScalar *beta) +{ + btVector3 p; + p[0] = pb[0] - pa[0]; + p[1] = pb[1] - pa[1]; + p[2] = pb[2] - pa[2]; + btScalar uaub = dDOT(ua,ub); + btScalar q1 = dDOT(ua,p); + btScalar q2 = -dDOT(ub,p); + btScalar d = 1-uaub*uaub; + if (d <= btScalar(0.0001f)) { + // @@@ this needs to be made more robust + *alpha = 0; + *beta = 0; + } + else { + d = 1.f/d; + *alpha = (q1 + uaub*q2)*d; + *beta = (uaub*q1 + q2)*d; + } +} + + + +// find all the intersection points between the 2D rectangle with vertices +// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]), +// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]). +// +// the intersection points are returned as x,y pairs in the 'ret' array. +// the number of intersection points is returned by the function (this will +// be in the range 0 to 8). + +static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16]) +{ + // q (and r) contain nq (and nr) coordinate points for the current (and + // chopped) polygons + int nq=4,nr=0; + btScalar buffer[16]; + btScalar *q = p; + btScalar *r = ret; + for (int dir=0; dir <= 1; dir++) { + // direction notation: xy[0] = x axis, xy[1] = y axis + for (int sign=-1; sign <= 1; sign += 2) { + // chop q along the line xy[dir] = sign*h[dir] + btScalar *pq = q; + btScalar *pr = r; + nr = 0; + for (int i=nq; i > 0; i--) { + // go through all points in q and all lines between adjacent points + if (sign*pq[dir] < h[dir]) { + // this point is inside the chopping line + pr[0] = pq[0]; + pr[1] = pq[1]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + btScalar *nextq = (i > 1) ? pq+2 : q; + if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) { + // this line crosses the chopping line + pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) / + (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]); + pr[dir] = sign*h[dir]; + pr += 2; + nr++; + if (nr & 8) { + q = r; + goto done; + } + } + pq += 2; + } + q = r; + r = (q==ret) ? buffer : ret; + nq = nr; + } + } + done: + if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar)); + return nr; +} + + +#define M__PI 3.14159265f + +// given n points in the plane (array p, of size 2*n), generate m points that +// best represent the whole set. the definition of 'best' here is not +// predetermined - the idea is to select points that give good box-box +// collision detection behavior. the chosen point indexes are returned in the +// array iret (of size m). 'i0' is always the first entry in the array. +// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be +// in the range [0..n-1]. + +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]); +void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[]) +{ + // compute the centroid of the polygon in cx,cy + int i,j; + btScalar a,cx,cy,q; + if (n==1) { + cx = p[0]; + cy = p[1]; + } + else if (n==2) { + cx = btScalar(0.5)*(p[0] + p[2]); + cy = btScalar(0.5)*(p[1] + p[3]); + } + else { + a = 0; + cx = 0; + cy = 0; + for (i=0; i<(n-1); i++) { + q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1]; + a += q; + cx += q*(p[i*2]+p[i*2+2]); + cy += q*(p[i*2+1]+p[i*2+3]); + } + q = p[n*2-2]*p[1] - p[0]*p[n*2-1]; + if (btFabs(a+q) > SIMD_EPSILON) + { + a = 1.f/(btScalar(3.0)*(a+q)); + } else + { + a=1e30f; + } + cx = a*(cx + q*(p[n*2-2]+p[0])); + cy = a*(cy + q*(p[n*2-1]+p[1])); + } + + // compute the angle of each point w.r.t. the centroid + btScalar A[8]; + for (i=0; i M__PI) a -= 2*M__PI; + btScalar maxdiff=1e9,diff; + + *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0 + + for (i=0; i M__PI) diff = 2*M__PI - diff; + if (diff < maxdiff) { + maxdiff = diff; + *iret = i; + } + } + } +#if defined(DEBUG) || defined (_DEBUG) + btAssert (*iret != i0); // ensure iret got set +#endif + avail[*iret] = 0; + iret++; + } +} + + + +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output); +int dBoxBox2 (const btVector3& p1, const dMatrix3 R1, + const btVector3& side1, const btVector3& p2, + const dMatrix3 R2, const btVector3& side2, + btVector3& normal, btScalar *depth, int *return_code, + int maxc, dContactGeom * /*contact*/, int /*skip*/,btDiscreteCollisionDetectorInterface::Result& output) +{ + const btScalar fudge_factor = btScalar(1.05); + btVector3 p,pp,normalC; + const btScalar *normalR = 0; + btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33, + Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l; + int i,j,invert_normal,code; + + // get vector from centers of box 1 to box 2, relative to box 1 + p = p2 - p1; + dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1 + + // get side lengths / 2 + A[0] = side1[0]*btScalar(0.5); + A[1] = side1[1]*btScalar(0.5); + A[2] = side1[2]*btScalar(0.5); + B[0] = side2[0]*btScalar(0.5); + B[1] = side2[1]*btScalar(0.5); + B[2] = side2[2]*btScalar(0.5); + + // Rij is R1'*R2, i.e. the relative rotation between R1 and R2 + R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2); + R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2); + R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2); + + Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13); + Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23); + Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33); + + // for all 15 possible separating axes: + // * see if the axis separates the boxes. if so, return 0. + // * find the depth of the penetration along the separating axis (s2) + // * if this is the largest depth so far, record it. + // the normal vector will be set to the separating axis with the smallest + // depth. note: normalR is set to point to a column of R1 or R2 if that is + // the smallest depth normal so far. otherwise normalR is 0 and normalC is + // set to a vector relative to body 1. invert_normal is 1 if the sign of + // the normal should be flipped. + +#define TST(expr1,expr2,norm,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis = u1,u2,u3 + TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1); + TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2); + TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3); + + // separating axis = v1,v2,v3 + TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4); + TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5); + TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6); + + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to box 1. +#undef TST +#define TST(expr1,expr2,n1,n2,n3,cc) \ + s2 = btFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2*fudge_factor > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } \ + } + + // separating axis = u1 x (v1,v2,v3) + TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7); + TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8); + TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9); + + // separating axis = u2 x (v1,v2,v3) + TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10); + TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11); + TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12); + + // separating axis = u3 x (v1,v2,v3) + TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13); + TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14); + TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15); + +#undef TST + + if (!code) return 0; + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + dMULTIPLY0_331 (normal,R1,normalC); + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (code > 6) { + // an edge from box 1 touches an edge from box 2. + // find a point pa on the intersecting edge of box 1 + btVector3 pa; + btScalar sign; + for (i=0; i<3; i++) pa[i] = p1[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0); + for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j]; + } + + // find a point pb on the intersecting edge of box 2 + btVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + for (j=0; j<3; j++) { + sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0); + for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j]; + } + + btScalar alpha,beta; + btVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4]; + + dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + { + + //contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]); + //contact[0].depth = *depth; + btVector3 pointInWorld; + +#ifdef USE_CENTER_POINT + for (i=0; i<3; i++) + pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5); + output.addContactPoint(-normal,pointInWorld,-*depth); +#else + output.addContactPoint(-normal,pb,-*depth); +#endif // + *return_code = code; + } + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). define face 'a' to be the reference + // face (i.e. the normal vector is perpendicular to this) and face 'b' to be + // the incident face (the closest face of the other box). + + const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb; + if (code <= 3) { + Ra = R1; + Rb = R2; + pa = p1; + pb = p2; + Sa = A; + Sb = B; + } + else { + Ra = R2; + Rb = R1; + pa = p2; + pb = p1; + Sa = B; + Sb = A; + } + + // nr = normal vector of reference face dotted with axes of incident box. + // anr = absolute values of nr. + btVector3 normal2,nr,anr; + if (code <= 3) { + normal2[0] = normal[0]; + normal2[1] = normal[1]; + normal2[2] = normal[2]; + } + else { + normal2[0] = -normal[0]; + normal2[1] = -normal[1]; + normal2[2] = -normal[2]; + } + dMULTIPLY1_331 (nr,Rb,normal2); + anr[0] = btFabs (nr[0]); + anr[1] = btFabs (nr[1]); + anr[2] = btFabs (nr[2]); + + // find the largest compontent of anr: this corresponds to the normal + // for the indident face. the other axis numbers of the indicent face + // are stored in a1,a2. + int lanr,a1,a2; + if (anr[1] > anr[0]) { + if (anr[1] > anr[2]) { + a1 = 0; + lanr = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + else { + if (anr[0] > anr[2]) { + lanr = 0; + a1 = 1; + a2 = 2; + } + else { + a1 = 0; + a2 = 1; + lanr = 2; + } + } + + // compute center point of incident face, in reference-face coordinates + btVector3 center; + if (nr[lanr] < 0) { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr]; + } + else { + for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr]; + } + + // find the normal and non-normal axis numbers of the reference box + int codeN,code1,code2; + if (code <= 3) codeN = code-1; else codeN = code-4; + if (codeN==0) { + code1 = 1; + code2 = 2; + } + else if (codeN==1) { + code1 = 0; + code2 = 2; + } + else { + code1 = 0; + code2 = 1; + } + + // find the four corners of the incident face, in reference-face coordinates + btScalar quad[8]; // 2D coordinate of incident face (x,y pairs) + btScalar c1,c2,m11,m12,m21,m22; + c1 = dDOT14 (center,Ra+code1); + c2 = dDOT14 (center,Ra+code2); + // optimize this? - we have already computed this data above, but it is not + // stored in an easy-to-index format. for now it's quicker just to recompute + // the four dot products. + m11 = dDOT44 (Ra+code1,Rb+a1); + m12 = dDOT44 (Ra+code1,Rb+a2); + m21 = dDOT44 (Ra+code2,Rb+a1); + m22 = dDOT44 (Ra+code2,Rb+a2); + { + btScalar k1 = m11*Sb[a1]; + btScalar k2 = m21*Sb[a1]; + btScalar k3 = m12*Sb[a2]; + btScalar k4 = m22*Sb[a2]; + quad[0] = c1 - k1 - k3; + quad[1] = c2 - k2 - k4; + quad[2] = c1 - k1 + k3; + quad[3] = c2 - k2 + k4; + quad[4] = c1 + k1 + k3; + quad[5] = c2 + k2 + k4; + quad[6] = c1 + k1 - k3; + quad[7] = c2 + k2 - k4; + } + + // find the size of the reference face + btScalar rect[2]; + rect[0] = Sa[code1]; + rect[1] = Sa[code2]; + + // intersect the incident and reference faces + btScalar ret[16]; + int n = intersectRectQuad2 (rect,quad,ret); + if (n < 1) return 0; // this should never happen + + // convert the intersection points into reference-face coordinates, + // and compute the contact position and depth for each point. only keep + // those points that have a positive (penetrating) depth. delete points in + // the 'ret' array as necessary so that 'point' and 'ret' correspond. + btScalar point[3*8]; // penetrating contact points + btScalar dep[8]; // depths for those points + btScalar det1 = 1.f/(m11*m22 - m12*m21); + m11 *= det1; + m12 *= det1; + m21 *= det1; + m22 *= det1; + int cnum = 0; // number of penetrating contact points found + for (j=0; j < n; j++) { + btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2); + btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2); + for (i=0; i<3; i++) point[cnum*3+i] = + center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2]; + dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3); + if (dep[cnum] >= 0) { + ret[cnum*2] = ret[j*2]; + ret[cnum*2+1] = ret[j*2+1]; + cnum++; + } + } + if (cnum < 1) return 0; // this should never happen + + // we can't generate more contacts than we actually have + if (maxc > cnum) maxc = cnum; + if (maxc < 1) maxc = 1; + + if (cnum <= maxc) { + // we have less contacts than we need, so we use them all + for (j=0; j < cnum; j++) { + + //AddContactPoint... + + //dContactGeom *con = CONTACT(contact,skip*j); + //for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i]; + //con->depth = dep[j]; + + btVector3 pointInWorld; + for (i=0; i<3; i++) + pointInWorld[i] = point[j*3+i] + pa[i]; + output.addContactPoint(-normal,pointInWorld,-dep[j]); + + } + } + else { + // we have more contacts than are wanted, some of them must be culled. + // find the deepest point, it is always the first contact. + int i1 = 0; + btScalar maxdepth = dep[0]; + for (i=1; i maxdepth) { + maxdepth = dep[i]; + i1 = i; + } + } + + int iret[8]; + cullPoints2 (cnum,ret,maxc,i1,iret); + + for (j=0; j < maxc; j++) { +// dContactGeom *con = CONTACT(contact,skip*j); + // for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i]; + // con->depth = dep[iret[j]]; + + btVector3 posInWorld; + for (i=0; i<3; i++) + posInWorld[i] = point[iret[j]*3+i] + pa[i]; + output.addContactPoint(-normal,posInWorld,-dep[iret[j]]); + } + cnum = maxc; + } + + *return_code = code; + return cnum; +} + +void btBoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* /*debugDraw*/,bool /*swapResults*/) +{ + + const btTransform& transformA = input.m_transformA; + const btTransform& transformB = input.m_transformB; + + int skip = 0; + dContactGeom *contact = 0; + + dMatrix3 R1; + dMatrix3 R2; + + for (int j=0;j<3;j++) + { + R1[0+4*j] = transformA.getBasis()[j].x(); + R2[0+4*j] = transformB.getBasis()[j].x(); + + R1[1+4*j] = transformA.getBasis()[j].y(); + R2[1+4*j] = transformB.getBasis()[j].y(); + + + R1[2+4*j] = transformA.getBasis()[j].z(); + R2[2+4*j] = transformB.getBasis()[j].z(); + + } + + + + btVector3 normal; + btScalar depth; + int return_code; + int maxc = 4; + + + dBoxBox2 (transformA.getOrigin(), + R1, + 2.f*m_box1->getHalfExtentsWithMargin(), + transformB.getOrigin(), + R2, + 2.f*m_box2->getHalfExtentsWithMargin(), + normal, &depth, &return_code, + maxc, contact, skip, + output + ); + +} diff --git a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index eacc90e85..605294d47 100644 --- a/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -1,44 +1,44 @@ -/* - * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith - * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. - * All rights reserved. Email: russ@q12.org Web: www.q12.org - -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef BOX_BOX_DETECTOR_H -#define BOX_BOX_DETECTOR_H - - -class btBoxShape; -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" - - -/// btBoxBoxDetector wraps the ODE box-box collision detector -/// re-distributed under the Zlib license with permission from Russell L. Smith -struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface -{ - btBoxShape* m_box1; - btBoxShape* m_box2; - -public: - - btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2); - - virtual ~btBoxBoxDetector() {}; - - virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); - -}; - -#endif //BT_BOX_BOX_DETECTOR_H +/* + * Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef BOX_BOX_DETECTOR_H +#define BOX_BOX_DETECTOR_H + + +class btBoxShape; +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" + + +/// btBoxBoxDetector wraps the ODE box-box collision detector +/// re-distributed under the Zlib license with permission from Russell L. Smith +struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface +{ + btBoxShape* m_box1; + btBoxShape* m_box2; + +public: + + btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2); + + virtual ~btBoxBoxDetector() {}; + + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); + +}; + +#endif //BT_BOX_BOX_DETECTOR_H diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index 7bb5910c8..a7b3b163d 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -1,155 +1,155 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btConvexPlaneCollisionAlgorithm.h" - -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" - -//#include - -btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) -: btCollisionAlgorithm(ci), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped), -m_numPerturbationIterations(numPerturbationIterations), -m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) -{ - btCollisionObject* convexObj = m_isSwapped? col1 : col0; - btCollisionObject* planeObj = m_isSwapped? col0 : col1; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); - m_ownManifold = true; - } -} - - -btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - btCollisionObject* convexObj = m_isSwapped? body1 : body0; - btCollisionObject* planeObj = m_isSwapped? body0: body1; - - btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - - bool hasCollision = false; - const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); - - btTransform convexWorldTransform = convexObj->getWorldTransform(); - btTransform convexInPlaneTrans; - convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform; - //now perturbe the convex-world transform - convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); - btTransform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform(); - - btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); - - btVector3 vtxInPlane = convexInPlaneTrans(vtx); - btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); - - btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; - btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; - - hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); - resultOut->setPersistentManifold(m_manifoldPtr); - if (hasCollision) - { - /// report a contact. internally this will be kept persistent, and contact reduction is done - btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; - btVector3 pOnB = vtxInPlaneWorld; - resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); - } -} - - -void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - if (!m_manifoldPtr) - return; - - btCollisionObject* convexObj = m_isSwapped? body1 : body0; - btCollisionObject* planeObj = m_isSwapped? body0: body1; - - btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); - btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - - bool hasCollision = false; - const btVector3& planeNormal = planeShape->getPlaneNormal(); - const btScalar& planeConstant = planeShape->getPlaneConstant(); - - //first perform a collision query with the non-perturbated collision objects - { - btQuaternion rotq(0,0,0,1); - collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); - } - - if (resultOut->getPersistentManifold()->getNumContacts()getAngularMotionDisc(); - perturbeAngle = gContactBreakingThreshold / radius; - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; - - btQuaternion perturbeRot(v0,perturbeAngle); - for (int i=0;igetNumContacts()) - { - resultOut->refreshContactPoints(); - } - } -} - -btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvexPlaneCollisionAlgorithm.h" + +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" + +//#include + +btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) +: btCollisionAlgorithm(ci), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped), +m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + btCollisionObject* convexObj = m_isSwapped? col1 : col0; + btCollisionObject* planeObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); + m_ownManifold = true; + } +} + + +btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + btTransform convexWorldTransform = convexObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform; + //now perturbe the convex-world transform + convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); + btTransform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; + + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) + { + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); + } +} + + +void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + if (!m_manifoldPtr) + return; + + btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* planeObj = m_isSwapped? body0: body1; + + btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + //first perform a collision query with the non-perturbated collision objects + { + btQuaternion rotq(0,0,0,1); + collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); + } + + if (resultOut->getPersistentManifold()->getNumContacts()getAngularMotionDisc(); + perturbeAngle = gContactBreakingThreshold / radius; + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btQuaternion perturbeRot(v0,perturbeAngle); + for (int i=0;igetNumContacts()) + { + resultOut->refreshContactPoints(); + } + } +} + +btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index dcfd36746..368ca71dd 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -1,84 +1,84 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H -#define CONVEX_PLANE_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" - -/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - -public: - - btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); - - virtual ~btConvexPlaneCollisionAlgorithm(); - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - int m_numPerturbationIterations; - int m_minimumPointsPerturbationThreshold; - - CreateFunc() - : m_numPerturbationIterations(3), - m_minimumPointsPerturbationThreshold(3) - { - } - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); - if (!m_swapped) - { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } else - { - return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); - } - } - }; - -}; - -#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H +#define CONVEX_PLANE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + +public: + + btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); + + virtual ~btConvexPlaneCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc() + : m_numPerturbationIterations(3), + m_minimumPointsPerturbationThreshold(3) + { + } + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } else + { + return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + } + }; + +}; + +#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionDispatch/btGhostObject.cpp b/src/BulletCollision/CollisionDispatch/btGhostObject.cpp index 8b994ca5a..a37e10e67 100644 --- a/src/BulletCollision/CollisionDispatch/btGhostObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btGhostObject.cpp @@ -1,170 +1,170 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btGhostObject.h" -#include "btCollisionWorld.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "LinearMath/btAabbUtil2.h" - -btGhostObject::btGhostObject() -{ - m_internalType = CO_GHOST_OBJECT; -} - -btGhostObject::~btGhostObject() -{ - ///btGhostObject should have been removed from the world, so no overlapping objects - btAssert(!m_overlappingObjects.size()); -} - - -void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btAssert(otherObject); - ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) - { - //not found - m_overlappingObjects.push_back(otherObject); - } -} - -void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (indexm_clientObject; - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (index==m_overlappingObjects.size()) - { - m_overlappingObjects.push_back(otherObject); - m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); - } -} - -void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) -{ - btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; - btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); - btAssert(actualThisProxy); - - btAssert(otherObject); - int index = m_overlappingObjects.findLinearSearch(otherObject); - if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); - } -} - - -void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const -{ - btTransform convexFromTrans,convexToTrans; - convexFromTrans = convexFromWorld; - convexToTrans = convexToWorld; - btVector3 castShapeAabbMin, castShapeAabbMax; - /* Compute AABB that encompasses angular movement */ - { - btVector3 linVel, angVel; - btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); - btTransform R; - R.setIdentity (); - R.setRotation (convexFromTrans.getRotation()); - castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); - } - - /// go over all objects, and if the ray intersects their aabb + cast shape aabb, - // do a ray-shape query using convexCaster (CCD) - int i; - for (i=0;igetBroadphaseHandle())) { - //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing - btVector3 hitNormal; - if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) - { - btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback, - allowedCcdPenetration); - } - } - } - -} - -void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const -{ - btTransform rayFromTrans; - rayFromTrans.setIdentity(); - rayFromTrans.setOrigin(rayFromWorld); - btTransform rayToTrans; - rayToTrans.setIdentity(); - rayToTrans.setOrigin(rayToWorld); - - - int i; - for (i=0;igetBroadphaseHandle())) - { - btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); - } - } -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btGhostObject.h" +#include "btCollisionWorld.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btAabbUtil2.h" + +btGhostObject::btGhostObject() +{ + m_internalType = CO_GHOST_OBJECT; +} + +btGhostObject::~btGhostObject() +{ + ///btGhostObject should have been removed from the world, so no overlapping objects + btAssert(!m_overlappingObjects.size()); +} + + +void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + //not found + m_overlappingObjects.push_back(otherObject); + } +} + +void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (indexm_clientObject; + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (index==m_overlappingObjects.size()) + { + m_overlappingObjects.push_back(otherObject); + m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); + } +} + +void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) +{ + btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; + btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); + btAssert(actualThisProxy); + + btAssert(otherObject); + int index = m_overlappingObjects.findLinearSearch(otherObject); + if (indexremoveOverlappingPair(actualThisProxy,otherProxy,dispatcher); + } +} + + +void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const +{ + btTransform convexFromTrans,convexToTrans; + convexFromTrans = convexFromWorld; + convexToTrans = convexToWorld; + btVector3 castShapeAabbMin, castShapeAabbMax; + /* Compute AABB that encompasses angular movement */ + { + btVector3 linVel, angVel; + btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); + btTransform R; + R.setIdentity (); + R.setRotation (convexFromTrans.getRotation()); + castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); + } + + /// go over all objects, and if the ray intersects their aabb + cast shape aabb, + // do a ray-shape query using convexCaster (CCD) + int i; + for (i=0;igetBroadphaseHandle())) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback, + allowedCcdPenetration); + } + } + } + +} + +void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const +{ + btTransform rayFromTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + btTransform rayToTrans; + rayToTrans.setIdentity(); + rayToTrans.setOrigin(rayToWorld); + + + int i; + for (i=0;igetBroadphaseHandle())) + { + btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } + } +} + diff --git a/src/BulletCollision/CollisionDispatch/btGhostObject.h b/src/BulletCollision/CollisionDispatch/btGhostObject.h index d1e4d5cc8..95b575024 100644 --- a/src/BulletCollision/CollisionDispatch/btGhostObject.h +++ b/src/BulletCollision/CollisionDispatch/btGhostObject.h @@ -1,174 +1,174 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_GHOST_OBJECT_H -#define BT_GHOST_OBJECT_H - - -#include "btCollisionObject.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" -#include "LinearMath/btAlignedAllocator.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "btCollisionWorld.h" - -class btConvexShape; - -class btDispatcher; - -///The btGhostObject can keep track of all objects that are overlapping -///By default, this overlap is based on the AABB -///This is useful for creating a character controller, collision sensors/triggers, explosions etc. -///We plan on adding rayTest and other queries for the btGhostObject -ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject -{ -protected: - - btAlignedObjectArray m_overlappingObjects; - -public: - - btGhostObject(); - - virtual ~btGhostObject(); - - void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; - - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; - - ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); - ///this method is mainly for expert/internal use only. - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); - - int getNumOverlappingObjects() const - { - return m_overlappingObjects.size(); - } - - btCollisionObject* getOverlappingObject(int index) - { - return m_overlappingObjects[index]; - } - - const btCollisionObject* getOverlappingObject(int index) const - { - return m_overlappingObjects[index]; - } - - btAlignedObjectArray& getOverlappingPairs() - { - return m_overlappingObjects; - } - - const btAlignedObjectArray getOverlappingPairs() const - { - return m_overlappingObjects; - } - - // - // internal cast - // - - static const btGhostObject* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_GHOST_OBJECT) - return (const btGhostObject*)colObj; - return 0; - } - static btGhostObject* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_GHOST_OBJECT) - return (btGhostObject*)colObj; - return 0; - } - -}; - -class btPairCachingGhostObject : public btGhostObject -{ - btHashedOverlappingPairCache* m_hashPairCache; - -public: - - btPairCachingGhostObject(); - - virtual ~btPairCachingGhostObject(); - - ///this method is mainly for expert/internal use only. - virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); - - virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); - - btHashedOverlappingPairCache* getOverlappingPairCache() - { - return m_hashPairCache; - } - -}; - - - -///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. -class btGhostPairCallback : public btOverlappingPairCallback -{ - -public: - btGhostPairCallback() - { - } - - virtual ~btGhostPairCallback() - { - - } - - virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) - { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); - if (ghost0) - ghost0->addOverlappingObjectInternal(proxy1, proxy0); - if (ghost1) - ghost1->addOverlappingObjectInternal(proxy0, proxy1); - return 0; - } - - virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) - { - btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; - btGhostObject* ghost0 = btGhostObject::upcast(colObj0); - btGhostObject* ghost1 = btGhostObject::upcast(colObj1); - if (ghost0) - ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); - if (ghost1) - ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); - return 0; - } - - virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) - { - btAssert(0); - //need to keep track of all ghost objects and call them here - //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); - } - - - -}; - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_GHOST_OBJECT_H +#define BT_GHOST_OBJECT_H + + +#include "btCollisionObject.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" +#include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "btCollisionWorld.h" + +class btConvexShape; + +class btDispatcher; + +///The btGhostObject can keep track of all objects that are overlapping +///By default, this overlap is based on the AABB +///This is useful for creating a character controller, collision sensors/triggers, explosions etc. +///We plan on adding rayTest and other queries for the btGhostObject +ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject +{ +protected: + + btAlignedObjectArray m_overlappingObjects; + +public: + + btGhostObject(); + + virtual ~btGhostObject(); + + void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const; + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + ///this method is mainly for expert/internal use only. + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + int getNumOverlappingObjects() const + { + return m_overlappingObjects.size(); + } + + btCollisionObject* getOverlappingObject(int index) + { + return m_overlappingObjects[index]; + } + + const btCollisionObject* getOverlappingObject(int index) const + { + return m_overlappingObjects[index]; + } + + btAlignedObjectArray& getOverlappingPairs() + { + return m_overlappingObjects; + } + + const btAlignedObjectArray getOverlappingPairs() const + { + return m_overlappingObjects; + } + + // + // internal cast + // + + static const btGhostObject* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (const btGhostObject*)colObj; + return 0; + } + static btGhostObject* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_GHOST_OBJECT) + return (btGhostObject*)colObj; + return 0; + } + +}; + +class btPairCachingGhostObject : public btGhostObject +{ + btHashedOverlappingPairCache* m_hashPairCache; + +public: + + btPairCachingGhostObject(); + + virtual ~btPairCachingGhostObject(); + + ///this method is mainly for expert/internal use only. + virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); + + virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); + + btHashedOverlappingPairCache* getOverlappingPairCache() + { + return m_hashPairCache; + } + +}; + + + +///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. +class btGhostPairCallback : public btOverlappingPairCallback +{ + +public: + btGhostPairCallback() + { + } + + virtual ~btGhostPairCallback() + { + + } + + virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->addOverlappingObjectInternal(proxy1, proxy0); + if (ghost1) + ghost1->addOverlappingObjectInternal(proxy0, proxy1); + return 0; + } + + virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) + { + btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; + btGhostObject* ghost0 = btGhostObject::upcast(colObj0); + btGhostObject* ghost1 = btGhostObject::upcast(colObj1); + if (ghost0) + ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); + if (ghost1) + ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) + { + btAssert(0); + //need to keep track of all ghost objects and call them here + //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); + } + + + +}; + #endif \ No newline at end of file diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 4fcb7ec1c..0328d0f73 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -1,390 +1,390 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "LinearMath/btScalar.h" -#include "btSimulationIslandManager.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" - -//#include -#include "LinearMath/btQuickprof.h" - -btSimulationIslandManager::btSimulationIslandManager(): -m_splitIslands(true) -{ -} - -btSimulationIslandManager::~btSimulationIslandManager() -{ -} - - -void btSimulationIslandManager::initUnionFind(int n) -{ - m_unionFind.reset(n); -} - - -void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) -{ - - { - - for (int i=0;igetPairCache()->getNumOverlappingPairs();i++) - { - btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); - const btBroadphasePair& collisionPair = pairPtr[i]; - btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; - btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; - - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) - { - - m_unionFind.unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } - } - } -} - - -void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) -{ - - initUnionFind( int (colWorld->getCollisionObjectArray().size())); - - // put the index into m_controllers into m_tag - { - - int index = 0; - int i; - for (i=0;igetCollisionObjectArray().size(); i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - collisionObject->setIslandTag(index); - collisionObject->setCompanionId(-1); - collisionObject->setHitFraction(btScalar(1.)); - index++; - - } - } - // do the union find - - findUnions(dispatcher,colWorld); - - - -} - - - - -void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) -{ - // put the islandId ('find' value) into m_tag - { - - - int index = 0; - int i; - for (i=0;igetCollisionObjectArray().size();i++) - { - btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; - if (!collisionObject->isStaticOrKinematicObject()) - { - collisionObject->setIslandTag( m_unionFind.find(index) ); - collisionObject->setCompanionId(-1); - } else - { - collisionObject->setIslandTag(-1); - collisionObject->setCompanionId(-2); - } - index++; - } - } -} - -inline int getIslandId(const btPersistentManifold* lhs) -{ - int islandId; - const btCollisionObject* rcolObj0 = static_cast(lhs->getBody0()); - const btCollisionObject* rcolObj1 = static_cast(lhs->getBody1()); - islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); - return islandId; - -} - - - -/// function object that routes calls to operator< -class btPersistentManifoldSortPredicate -{ - public: - - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) - { - return getIslandId(lhs) < getIslandId(rhs); - } -}; - - -void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) -{ - - BT_PROFILE("islandUnionFindAndQuickSort"); - - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - m_islandmanifold.resize(0); - - //we are going to sort the unionfind array, and store the element id in the size - //afterwards, we clean unionfind, to make sure no-one uses it anymore - - getUnionFind().sortIslands(); - int numElem = getUnionFind().getNumElements(); - - int endIslandIndex=1; - int startIslandIndex; - - - //update the sleeping state for bodies, if all are sleeping - for ( startIslandIndex=0;startIslandIndexgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - if (colObj0->getIslandTag() == islandId) - { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) - { - allSleeping = false; - } - } - } - - - if (allSleeping) - { - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - colObj0->setActivationState( ISLAND_SLEEPING ); - } - } - } else - { - - int idx; - for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) - { -// printf("error in island management\n"); - } - - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); - - if (colObj0->getIslandTag() == islandId) - { - if ( colObj0->getActivationState() == ISLAND_SLEEPING) - { - colObj0->setActivationState( WANTS_DEACTIVATION); - colObj0->setDeactivationTime(0.f); - } - } - } - } - } - - - int i; - int maxNumManifolds = dispatcher->getNumManifolds(); - -//#define SPLIT_ISLANDS 1 -//#ifdef SPLIT_ISLANDS - - -//#endif //SPLIT_ISLANDS - - - for (i=0;igetManifoldByIndexInternal(i); - - btCollisionObject* colObj0 = static_cast(manifold->getBody0()); - btCollisionObject* colObj1 = static_cast(manifold->getBody1()); - - ///@todo: check sleeping conditions! - if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || - ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) - { - - //kinematic objects don't merge islands, but wake up all connected objects - if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) - { - colObj1->activate(); - } - if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) - { - colObj0->activate(); - } - if(m_splitIslands) - { - //filtering for response - if (dispatcher->needsResponse(colObj0,colObj1)) - m_islandmanifold.push_back(manifold); - } - } - } -} - - - -///@todo: this is random access, it can be walked 'cache friendly'! -void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) -{ - btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); - - buildIslands(dispatcher,collisionWorld); - - int endIslandIndex=1; - int startIslandIndex; - int numElem = getUnionFind().getNumElements(); - - BT_PROFILE("processIslands"); - - if(!m_splitIslands) - { - btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); - int maxNumManifolds = dispatcher->getNumManifolds(); - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); - } - else - { - // Sort manifolds, based on islands - // Sort the vector using predicate and std::sort - //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); - - int numManifolds = int (m_islandmanifold.size()); - - //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); - - //now process all active islands (sets of manifolds for now) - - int startManifoldIndex = 0; - int endManifoldIndex = 1; - - //int islandId; - - - - // printf("Start Islands\n"); - - //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated - for ( startIslandIndex=0;startIslandIndexisActive()) - islandSleeping = true; - } - - - //find the accompanying contact manifold for this islandId - int numIslandManifolds = 0; - btPersistentManifold** startManifold = 0; - - if (startManifoldIndexProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); - // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); - } - - if (numIslandManifolds) - { - startManifoldIndex = endManifoldIndex; - } - - m_islandBodies.resize(0); - } - } // else if(!splitIslands) - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "LinearMath/btScalar.h" +#include "btSimulationIslandManager.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" + +//#include +#include "LinearMath/btQuickprof.h" + +btSimulationIslandManager::btSimulationIslandManager(): +m_splitIslands(true) +{ +} + +btSimulationIslandManager::~btSimulationIslandManager() +{ +} + + +void btSimulationIslandManager::initUnionFind(int n) +{ + m_unionFind.reset(n); +} + + +void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) +{ + + { + + for (int i=0;igetPairCache()->getNumOverlappingPairs();i++) + { + btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); + const btBroadphasePair& collisionPair = pairPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; + btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + + m_unionFind.unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } + } + } +} + + +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +{ + + initUnionFind( int (colWorld->getCollisionObjectArray().size())); + + // put the index into m_controllers into m_tag + { + + int index = 0; + int i; + for (i=0;igetCollisionObjectArray().size(); i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + collisionObject->setIslandTag(index); + collisionObject->setCompanionId(-1); + collisionObject->setHitFraction(btScalar(1.)); + index++; + + } + } + // do the union find + + findUnions(dispatcher,colWorld); + + + +} + + + + +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +{ + // put the islandId ('find' value) into m_tag + { + + + int index = 0; + int i; + for (i=0;igetCollisionObjectArray().size();i++) + { + btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; + if (!collisionObject->isStaticOrKinematicObject()) + { + collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setCompanionId(-1); + } else + { + collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); + } + index++; + } + } +} + +inline int getIslandId(const btPersistentManifold* lhs) +{ + int islandId; + const btCollisionObject* rcolObj0 = static_cast(lhs->getBody0()); + const btCollisionObject* rcolObj1 = static_cast(lhs->getBody1()); + islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); + return islandId; + +} + + + +/// function object that routes calls to operator< +class btPersistentManifoldSortPredicate +{ + public: + + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) + { + return getIslandId(lhs) < getIslandId(rhs); + } +}; + + +void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) +{ + + BT_PROFILE("islandUnionFindAndQuickSort"); + + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + m_islandmanifold.resize(0); + + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + + getUnionFind().sortIslands(); + int numElem = getUnionFind().getNumElements(); + + int endIslandIndex=1; + int startIslandIndex; + + + //update the sleeping state for bodies, if all are sleeping + for ( startIslandIndex=0;startIslandIndexgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) + { + if (colObj0->getActivationState()== ACTIVE_TAG) + { + allSleeping = false; + } + if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + { + allSleeping = false; + } + } + } + + + if (allSleeping) + { + int idx; + for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + colObj0->setActivationState( ISLAND_SLEEPING ); + } + } + } else + { + + int idx; + for (idx=startIslandIndex;idxgetIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { +// printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + if ( colObj0->getActivationState() == ISLAND_SLEEPING) + { + colObj0->setActivationState( WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); + } + } + } + } + } + + + int i; + int maxNumManifolds = dispatcher->getNumManifolds(); + +//#define SPLIT_ISLANDS 1 +//#ifdef SPLIT_ISLANDS + + +//#endif //SPLIT_ISLANDS + + + for (i=0;igetManifoldByIndexInternal(i); + + btCollisionObject* colObj0 = static_cast(manifold->getBody0()); + btCollisionObject* colObj1 = static_cast(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + colObj0->activate(); + } + if(m_splitIslands) + { + //filtering for response + if (dispatcher->needsResponse(colObj0,colObj1)) + m_islandmanifold.push_back(manifold); + } + } + } +} + + + +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + buildIslands(dispatcher,collisionWorld); + + int endIslandIndex=1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); + + BT_PROFILE("processIslands"); + + if(!m_splitIslands) + { + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + } + else + { + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + int numManifolds = int (m_islandmanifold.size()); + + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + + //now process all active islands (sets of manifolds for now) + + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + //int islandId; + + + + // printf("Start Islands\n"); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for ( startIslandIndex=0;startIslandIndexisActive()) + islandSleeping = true; + } + + + //find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + btPersistentManifold** startManifold = 0; + + if (startManifoldIndexProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); + // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + } + + if (numIslandManifolds) + { + startManifoldIndex = endManifoldIndex; + } + + m_islandBodies.resize(0); + } + } // else if(!splitIslands) + +} diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp index 9342919bf..8df876928 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -1,260 +1,260 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSphereBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -//#include - -btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) -: btActivatingCollisionAlgorithm(ci,col0,col1), -m_ownManifold(false), -m_manifoldPtr(mf), -m_isSwapped(isSwapped) -{ - btCollisionObject* sphereObj = m_isSwapped? col1 : col0; - btCollisionObject* boxObj = m_isSwapped? col0 : col1; - - if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) - { - m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); - m_ownManifold = true; - } -} - - -btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - - - -void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - (void)resultOut; - if (!m_manifoldPtr) - return; - - btCollisionObject* sphereObj = m_isSwapped? body1 : body0; - btCollisionObject* boxObj = m_isSwapped? body0 : body1; - - - btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape(); - - btVector3 normalOnSurfaceB; - btVector3 pOnBox,pOnSphere; - btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); - btScalar radius = sphere0->getRadius(); - - btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); - - resultOut->setPersistentManifold(m_manifoldPtr); - - if (dist < SIMD_EPSILON) - { - btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done - - resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist); - - } - - if (m_ownManifold) - { - if (m_manifoldPtr->getNumContacts()) - { - resultOut->refreshContactPoints(); - } - } - -} - -btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} - - -btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) -{ - - btScalar margins; - btVector3 bounds[2]; - btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); - - bounds[0] = -boxShape->getHalfExtentsWithoutMargin(); - bounds[1] = boxShape->getHalfExtentsWithoutMargin(); - - margins = boxShape->getMargin();//also add sphereShape margin? - - const btTransform& m44T = boxObj->getWorldTransform(); - - btVector3 boundsVec[2]; - btScalar fPenetration; - - boundsVec[0] = bounds[0]; - boundsVec[1] = bounds[1]; - - btVector3 marginsVec( margins, margins, margins ); - - // add margins - bounds[0] += marginsVec; - bounds[1] -= marginsVec; - - ///////////////////////////////////////////////// - - btVector3 tmp, prel, n[6], normal, v3P; - btScalar fSep = btScalar(10000000.0), fSepThis; - - n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); - n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); - n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); - n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); - n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); - n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); - - // convert point in local space - prel = m44T.invXform( sphereCenter); - - bool bFound = false; - - v3P = prel; - - for (int i=0;i<6;i++) - { - int j = i<3? 0:1; - if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) - { - v3P = v3P - n[i]*fSepThis; - bFound = true; - } - } - - // - - if ( bFound ) - { - bounds[0] = boundsVec[0]; - bounds[1] = boundsVec[1]; - - normal = (prel - v3P).normalize(); - pointOnBox = v3P + normal*margins; - v3PointOnSphere = prel - normal*fRadius; - - if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) ) - { - return btScalar(1.0); - } - - // transform back in world space - tmp = m44T( pointOnBox); - pointOnBox = tmp; - tmp = m44T( v3PointOnSphere); - v3PointOnSphere = tmp; - btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2(); - - //if this fails, fallback into deeper penetration case, below - if (fSeps2 > SIMD_EPSILON) - { - fSep = - btSqrt(fSeps2); - normal = (pointOnBox-v3PointOnSphere); - normal *= btScalar(1.)/fSep; - } - - return fSep; - } - - ////////////////////////////////////////////////// - // Deep penetration case - - fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] ); - - bounds[0] = boundsVec[0]; - bounds[1] = boundsVec[1]; - - if ( fPenetration <= btScalar(0.0) ) - return (fPenetration-margins); - else - return btScalar(1.0); -} - -btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) -{ - - btVector3 bounds[2]; - - bounds[0] = aabbMin; - bounds[1] = aabbMax; - - btVector3 p0, tmp, prel, n[6], normal; - btScalar fSep = btScalar(-10000000.0), fSepThis; - - // set p0 and normal to a default value to shup up GCC - p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - - n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); - n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); - n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); - n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); - n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); - n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); - - const btTransform& m44T = boxObj->getWorldTransform(); - - // convert point in local space - prel = m44T.invXform( sphereCenter); - - /////////// - - for (int i=0;i<6;i++) - { - int j = i<3 ? 0:1; - if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0); - if ( fSepThis > fSep ) - { - p0 = bounds[j]; normal = (btVector3&)n[i]; - fSep = fSepThis; - } - } - - pointOnBox = prel - normal*(normal.dot((prel-p0))); - v3PointOnSphere = pointOnBox + normal*fSep; - - // transform back in world space - tmp = m44T( pointOnBox); - pointOnBox = tmp; - tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; - normal = (pointOnBox-v3PointOnSphere).normalize(); - - return fSep; - -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSphereBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +//#include + +btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) +: btActivatingCollisionAlgorithm(ci,col0,col1), +m_ownManifold(false), +m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ + btCollisionObject* sphereObj = m_isSwapped? col1 : col0; + btCollisionObject* boxObj = m_isSwapped? col0 : col1; + + if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) + { + m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); + m_ownManifold = true; + } +} + + +btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + + + +void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + if (!m_manifoldPtr) + return; + + btCollisionObject* sphereObj = m_isSwapped? body1 : body0; + btCollisionObject* boxObj = m_isSwapped? body0 : body1; + + + btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape(); + + btVector3 normalOnSurfaceB; + btVector3 pOnBox,pOnSphere; + btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); + btScalar radius = sphere0->getRadius(); + + btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); + + resultOut->setPersistentManifold(m_manifoldPtr); + + if (dist < SIMD_EPSILON) + { + btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize(); + + /// report a contact. internally this will be kept persistent, and contact reduction is done + + resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist); + + } + + if (m_ownManifold) + { + if (m_manifoldPtr->getNumContacts()) + { + resultOut->refreshContactPoints(); + } + } + +} + +btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} + + +btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) +{ + + btScalar margins; + btVector3 bounds[2]; + btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); + + bounds[0] = -boxShape->getHalfExtentsWithoutMargin(); + bounds[1] = boxShape->getHalfExtentsWithoutMargin(); + + margins = boxShape->getMargin();//also add sphereShape margin? + + const btTransform& m44T = boxObj->getWorldTransform(); + + btVector3 boundsVec[2]; + btScalar fPenetration; + + boundsVec[0] = bounds[0]; + boundsVec[1] = bounds[1]; + + btVector3 marginsVec( margins, margins, margins ); + + // add margins + bounds[0] += marginsVec; + bounds[1] -= marginsVec; + + ///////////////////////////////////////////////// + + btVector3 tmp, prel, n[6], normal, v3P; + btScalar fSep = btScalar(10000000.0), fSepThis; + + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); + + // convert point in local space + prel = m44T.invXform( sphereCenter); + + bool bFound = false; + + v3P = prel; + + for (int i=0;i<6;i++) + { + int j = i<3? 0:1; + if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) + { + v3P = v3P - n[i]*fSepThis; + bFound = true; + } + } + + // + + if ( bFound ) + { + bounds[0] = boundsVec[0]; + bounds[1] = boundsVec[1]; + + normal = (prel - v3P).normalize(); + pointOnBox = v3P + normal*margins; + v3PointOnSphere = prel - normal*fRadius; + + if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) ) + { + return btScalar(1.0); + } + + // transform back in world space + tmp = m44T( pointOnBox); + pointOnBox = tmp; + tmp = m44T( v3PointOnSphere); + v3PointOnSphere = tmp; + btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2(); + + //if this fails, fallback into deeper penetration case, below + if (fSeps2 > SIMD_EPSILON) + { + fSep = - btSqrt(fSeps2); + normal = (pointOnBox-v3PointOnSphere); + normal *= btScalar(1.)/fSep; + } + + return fSep; + } + + ////////////////////////////////////////////////// + // Deep penetration case + + fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] ); + + bounds[0] = boundsVec[0]; + bounds[1] = boundsVec[1]; + + if ( fPenetration <= btScalar(0.0) ) + return (fPenetration-margins); + else + return btScalar(1.0); +} + +btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) +{ + + btVector3 bounds[2]; + + bounds[0] = aabbMin; + bounds[1] = aabbMax; + + btVector3 p0, tmp, prel, n[6], normal; + btScalar fSep = btScalar(-10000000.0), fSepThis; + + // set p0 and normal to a default value to shup up GCC + p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + normal.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + + n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) ); + n[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) ); + n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) ); + n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) ); + n[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) ); + n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); + + const btTransform& m44T = boxObj->getWorldTransform(); + + // convert point in local space + prel = m44T.invXform( sphereCenter); + + /////////// + + for (int i=0;i<6;i++) + { + int j = i<3 ? 0:1; + if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0); + if ( fSepThis > fSep ) + { + p0 = bounds[j]; normal = (btVector3&)n[i]; + fSep = fSepThis; + } + } + + pointOnBox = prel - normal*(normal.dot((prel-p0))); + v3PointOnSphere = pointOnBox + normal*fSep; + + // transform back in world space + tmp = m44T( pointOnBox); + pointOnBox = tmp; + tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; + normal = (pointOnBox-v3PointOnSphere).normalize(); + + return fSep; + +} + diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 6bded59b6..47111d1c4 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -1,75 +1,75 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H -#define SPHERE_BOX_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" - -/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_isSwapped; - -public: - - btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); - - virtual ~btSphereBoxCollisionAlgorithm(); - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); - - btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); - if (!m_swapped) - { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); - } else - { - return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); - } - } - }; - -}; - -#endif //SPHERE_BOX_COLLISION_ALGORITHM_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H +#define SPHERE_BOX_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_isSwapped; + +public: + + btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btSphereBoxCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); + + btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //SPHERE_BOX_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp index d08742bed..5c4e78fe5 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -1,105 +1,105 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSphereSphereCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1) -: btActivatingCollisionAlgorithm(ci,col0,col1), -m_ownManifold(false), -m_manifoldPtr(mf) -{ - if (!m_manifoldPtr) - { - m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); - m_ownManifold = true; - } -} - -btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - - if (!m_manifoldPtr) - return; - - resultOut->setPersistentManifold(m_manifoldPtr); - - btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape(); - btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); - - btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); - btScalar len = diff.length(); - btScalar radius0 = sphere0->getRadius(); - btScalar radius1 = sphere1->getRadius(); - -#ifdef CLEAR_MANIFOLD - m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting -#endif - - ///iff distance positive, don't generate a new contact - if ( len > (radius0+radius1)) - { -#ifndef CLEAR_MANIFOLD - resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - return; - } - ///distance (negative means penetration) - btScalar dist = len - (radius0+radius1); - - btVector3 normalOnSurfaceB(1,0,0); - if (len > SIMD_EPSILON) - { - normalOnSurfaceB = diff / len; - } - - ///point on A (worldspace) - ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; - ///point on B (worldspace) - btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; - - /// report a contact. internally this will be kept persistent, and contact reduction is done - - - resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); - -#ifndef CLEAR_MANIFOLD - resultOut->refreshContactPoints(); -#endif //CLEAR_MANIFOLD - -} - -btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)col0; - (void)col1; - (void)dispatchInfo; - (void)resultOut; - - //not yet - return btScalar(1.); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSphereSphereCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1) +: btActivatingCollisionAlgorithm(ci,col0,col1), +m_ownManifold(false), +m_manifoldPtr(mf) +{ + if (!m_manifoldPtr) + { + m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); + m_ownManifold = true; + } +} + +btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + + if (!m_manifoldPtr) + return; + + resultOut->setPersistentManifold(m_manifoldPtr); + + btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape(); + btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); + + btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); + btScalar len = diff.length(); + btScalar radius0 = sphere0->getRadius(); + btScalar radius1 = sphere1->getRadius(); + +#ifdef CLEAR_MANIFOLD + m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting +#endif + + ///iff distance positive, don't generate a new contact + if ( len > (radius0+radius1)) + { +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD + return; + } + ///distance (negative means penetration) + btScalar dist = len - (radius0+radius1); + + btVector3 normalOnSurfaceB(1,0,0); + if (len > SIMD_EPSILON) + { + normalOnSurfaceB = diff / len; + } + + ///point on A (worldspace) + ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; + ///point on B (worldspace) + btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; + + /// report a contact. internally this will be kept persistent, and contact reduction is done + + + resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + +#ifndef CLEAR_MANIFOLD + resultOut->refreshContactPoints(); +#endif //CLEAR_MANIFOLD + +} + +btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)col0; + (void)col1; + (void)dispatchInfo; + (void)resultOut; + + //not yet + return btScalar(1.); +} diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index e6b322376..7d07512ca 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -1,66 +1,66 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H -#define SPHERE_SPHERE_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "btCollisionDispatcher.h" - -class btPersistentManifold; - -/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -/// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - -public: - btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); - - btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btActivatingCollisionAlgorithm(ci) {} - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - virtual ~btSphereSphereCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); - return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1); - } - }; - -}; - -#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H +#define SPHERE_SPHERE_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" + +class btPersistentManifold; + +/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +/// Also provides the most basic sample for custom/user btCollisionAlgorithm +class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + virtual ~btSphereSphereCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); + return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 0e8e61cda..2d5efcf56 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -1,84 +1,84 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSphereTriangleCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "SphereTriangleDetector.h" - - -btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) -: btActivatingCollisionAlgorithm(ci,col0,col1), -m_ownManifold(false), -m_manifoldPtr(mf), -m_swapped(swapped) -{ - if (!m_manifoldPtr) - { - m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); - m_ownManifold = true; - } -} - -btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() -{ - if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } -} - -void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - if (!m_manifoldPtr) - return; - - btCollisionObject* sphereObj = m_swapped? col1 : col0; - btCollisionObject* triObj = m_swapped? col0 : col1; - - btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape(); - btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape(); - - /// report a contact. internally this will be kept persistent, and contact reduction is done - resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds - input.m_transformA = sphereObj->getWorldTransform(); - input.m_transformB = triObj->getWorldTransform(); - - bool swapResults = m_swapped; - - detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); - - if (m_ownManifold) - resultOut->refreshContactPoints(); - -} - -btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSphereTriangleCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "SphereTriangleDetector.h" + + +btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) +: btActivatingCollisionAlgorithm(ci,col0,col1), +m_ownManifold(false), +m_manifoldPtr(mf), +m_swapped(swapped) +{ + if (!m_manifoldPtr) + { + m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); + m_ownManifold = true; + } +} + +btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* sphereObj = m_swapped? col1 : col0; + btCollisionObject* triObj = m_swapped? col0 : col1; + + btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape(); + btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape(); + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds + input.m_transformA = sphereObj->getWorldTransform(); + input.m_transformB = triObj->getWorldTransform(); + + bool swapResults = m_swapped; + + detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); + + if (m_ownManifold) + resultOut->refreshContactPoints(); + +} + +btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index f184c700f..606c3635a 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -1,69 +1,69 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - -#include "btActivatingCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "btCollisionDispatcher.h" - -/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. -/// Other features are frame-coherency (persistent data) and collision response. -/// Also provides the most basic sample for custom/user btCollisionAlgorithm -class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - bool m_swapped; - -public: - btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); - - btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btActivatingCollisionAlgorithm(ci) {} - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - { - manifoldArray.push_back(m_manifoldPtr); - } - } - - virtual ~btSphereTriangleCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); - - return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); - } - }; - -}; - -#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + +#include "btActivatingCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "btCollisionDispatcher.h" + +/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +/// Also provides the most basic sample for custom/user btCollisionAlgorithm +class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_swapped; + +public: + btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); + + btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btActivatingCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + { + manifoldArray.push_back(m_manifoldPtr); + } + } + + virtual ~btSphereTriangleCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); + + return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); + } + }; + +}; + +#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H + diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index c00571372..60a96a542 100644 --- a/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -1,171 +1,171 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btCapsuleShape.h" - -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" -#include "LinearMath/btQuaternion.h" - -btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () -{ - m_shapeType = CAPSULE_SHAPE_PROXYTYPE; - m_upAxis = 1; - m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); -} - - - btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-1e30)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - btVector3 vtx; - btScalar newDot; - - btScalar radius = getRadius(); - - - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = getHalfHeight(); - - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = -getHalfHeight(); - - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - - return supVec; - -} - - void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - - - btScalar radius = getRadius(); - - for (int j=0;j maxDot) - { - maxDot = newDot; - supportVerticesOut[j] = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supportVerticesOut[j] = vtx; - } - } - - } -} - - -void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - //as an approximation, take the inertia of the box that bounds the spheres - - btTransform ident; - ident.setIdentity(); - - - btScalar radius = getRadius(); - - btVector3 halfExtents(radius,radius,radius); - halfExtents[getUpAxis()]+=getHalfHeight(); - - btScalar margin = CONVEX_DISTANCE_MARGIN; - - btScalar lx=btScalar(2.)*(halfExtents[0]+margin); - btScalar ly=btScalar(2.)*(halfExtents[1]+margin); - btScalar lz=btScalar(2.)*(halfExtents[2]+margin); - const btScalar x2 = lx*lx; - const btScalar y2 = ly*ly; - const btScalar z2 = lz*lz; - const btScalar scaledmass = mass * btScalar(.08333333); - - inertia[0] = scaledmass * (y2+z2); - inertia[1] = scaledmass * (x2+z2); - inertia[2] = scaledmass * (x2+y2); - -} - -btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) -{ - m_upAxis = 0; - m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); -} - - - - - - -btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) -{ - m_upAxis = 2; - m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); -} - - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btCapsuleShape.h" + +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btQuaternion.h" + +btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () +{ + m_shapeType = CAPSULE_SHAPE_PROXYTYPE; + m_upAxis = 1; + m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); +} + + + btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + btVector3 vtx; + btScalar newDot; + + btScalar radius = getRadius(); + + + { + btVector3 pos(0,0,0); + pos[getUpAxis()] = getHalfHeight(); + + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,0,0); + pos[getUpAxis()] = -getHalfHeight(); + + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + + void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + + + btScalar radius = getRadius(); + + for (int j=0;j maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + { + btVector3 pos(0,0,0); + pos[getUpAxis()] = -getHalfHeight(); + vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + + } +} + + +void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + //as an approximation, take the inertia of the box that bounds the spheres + + btTransform ident; + ident.setIdentity(); + + + btScalar radius = getRadius(); + + btVector3 halfExtents(radius,radius,radius); + halfExtents[getUpAxis()]+=getHalfHeight(); + + btScalar margin = CONVEX_DISTANCE_MARGIN; + + btScalar lx=btScalar(2.)*(halfExtents[0]+margin); + btScalar ly=btScalar(2.)*(halfExtents[1]+margin); + btScalar lz=btScalar(2.)*(halfExtents[2]+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(.08333333); + + inertia[0] = scaledmass * (y2+z2); + inertia[1] = scaledmass * (x2+z2); + inertia[2] = scaledmass * (x2+y2); + +} + +btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) +{ + m_upAxis = 0; + m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); +} + + + + + + +btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) +{ + m_upAxis = 2; + m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); +} + + + + diff --git a/src/BulletCollision/CollisionShapes/btCapsuleShape.h b/src/BulletCollision/CollisionShapes/btCapsuleShape.h index 5924acc23..828c1b3a5 100644 --- a/src/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/src/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -1,118 +1,118 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CAPSULE_SHAPE_H -#define BT_CAPSULE_SHAPE_H - -#include "btConvexInternalShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types - - -///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis. -///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres. -class btCapsuleShape : public btConvexInternalShape -{ -protected: - int m_upAxis; - -protected: - ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. - btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; - -public: - btCapsuleShape(btScalar radius,btScalar height); - - ///CollisionShape Interface - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - /// btConvexShape Interface - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - - virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const - { - btVector3 halfExtents(getRadius(),getRadius(),getRadius()); - halfExtents[m_upAxis] = getRadius() + getHalfHeight(); - halfExtents += btVector3(getMargin(),getMargin(),getMargin()); - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - } - - virtual const char* getName()const - { - return "CapsuleShape"; - } - - int getUpAxis() const - { - return m_upAxis; - } - - btScalar getRadius() const - { - int radiusAxis = (m_upAxis+2)%3; - return m_implicitShapeDimensions[radiusAxis]; - } - - btScalar getHalfHeight() const - { - return m_implicitShapeDimensions[m_upAxis]; - } - -}; - -///btCapsuleShapeX represents a capsule around the Z axis -///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -class btCapsuleShapeX : public btCapsuleShape -{ -public: - - btCapsuleShapeX(btScalar radius,btScalar height); - - //debugging - virtual const char* getName()const - { - return "CapsuleX"; - } - - - -}; - -///btCapsuleShapeZ represents a capsule around the Z axis -///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. -class btCapsuleShapeZ : public btCapsuleShape -{ -public: - btCapsuleShapeZ(btScalar radius,btScalar height); - - //debugging - virtual const char* getName()const - { - return "CapsuleZ"; - } - - -}; - - - -#endif //BT_CAPSULE_SHAPE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CAPSULE_SHAPE_H +#define BT_CAPSULE_SHAPE_H + +#include "btConvexInternalShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + + +///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis. +///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres. +class btCapsuleShape : public btConvexInternalShape +{ +protected: + int m_upAxis; + +protected: + ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. + btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; + +public: + btCapsuleShape(btScalar radius,btScalar height); + + ///CollisionShape Interface + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + /// btConvexShape Interface + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const + { + btVector3 halfExtents(getRadius(),getRadius(),getRadius()); + halfExtents[m_upAxis] = getRadius() + getHalfHeight(); + halfExtents += btVector3(getMargin(),getMargin(),getMargin()); + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + } + + virtual const char* getName()const + { + return "CapsuleShape"; + } + + int getUpAxis() const + { + return m_upAxis; + } + + btScalar getRadius() const + { + int radiusAxis = (m_upAxis+2)%3; + return m_implicitShapeDimensions[radiusAxis]; + } + + btScalar getHalfHeight() const + { + return m_implicitShapeDimensions[m_upAxis]; + } + +}; + +///btCapsuleShapeX represents a capsule around the Z axis +///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +class btCapsuleShapeX : public btCapsuleShape +{ +public: + + btCapsuleShapeX(btScalar radius,btScalar height); + + //debugging + virtual const char* getName()const + { + return "CapsuleX"; + } + + + +}; + +///btCapsuleShapeZ represents a capsule around the Z axis +///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps. +class btCapsuleShapeZ : public btCapsuleShape +{ +public: + btCapsuleShapeZ(btScalar radius,btScalar height); + + //debugging + virtual const char* getName()const + { + return "CapsuleZ"; + } + + +}; + + + +#endif //BT_CAPSULE_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp index 11007cdd7..4ab5d6929 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp @@ -1,156 +1,156 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "btConvexPointCloudShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" - -void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; - recalcLocalAabb(); -} - -#ifndef __SPU__ -btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - btScalar newDot,maxDot = btScalar(-1e30); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - - for (int i=0;i maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return supVec; -} - -void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - btScalar newDot; - //use 'w' component of supportVerticesOut? - { - for (int i=0;i supportVerticesOut[j][3]) - { - //WARNING: don't swap next lines, the w component would get overwritten! - supportVerticesOut[j] = vtx; - supportVerticesOut[j][3] = newDot; - } - } - } - - - -} - - - -btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const -{ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; - } - return supVertex; -} - - -#endif - - - - - - -//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection -//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo -int btConvexPointCloudShape::getNumVertices() const -{ - return m_numPoints; -} - -int btConvexPointCloudShape::getNumEdges() const -{ - return 0; -} - -void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const -{ - btAssert (0); -} - -void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const -{ - vtx = m_unscaledPoints[i]*m_localScaling; -} - -int btConvexPointCloudShape::getNumPlanes() const -{ - return 0; -} - -void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const -{ - - btAssert(0); -} - -//not yet -bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const -{ - btAssert(0); - return false; -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "btConvexPointCloudShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" + +void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; + recalcLocalAabb(); +} + +#ifndef __SPU__ +btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; +} + +void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + btScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + } + + + +} + + + +btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; +} + + +#endif + + + + + + +//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection +//Please note that you can debug-draw btConvexHullShape with the Raytracer Demo +int btConvexPointCloudShape::getNumVertices() const +{ + return m_numPoints; +} + +int btConvexPointCloudShape::getNumEdges() const +{ + return 0; +} + +void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const +{ + btAssert (0); +} + +void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const +{ + vtx = m_unscaledPoints[i]*m_localScaling; +} + +int btConvexPointCloudShape::getNumPlanes() const +{ + return 0; +} + +void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const +{ + + btAssert(0); +} + +//not yet +bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const +{ + btAssert(0); + return false; +} + diff --git a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h index 07cab59b0..7e1c13b29 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h @@ -1,96 +1,96 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H -#define BT_CONVEX_POINT_CLOUD_SHAPE_H - -#include "btPolyhedralConvexShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types -#include "LinearMath/btAlignedObjectArray.h" - -///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. -ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape -{ - btVector3* m_unscaledPoints; - int m_numPoints; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) - { - m_localScaling = localScaling; - m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; - m_unscaledPoints = points; - m_numPoints = numPoints; - - if (computeAabb) - recalcLocalAabb(); - } - - void setPoints (btVector3* points, int numPoints, bool computeAabb = true) - { - m_unscaledPoints = points; - m_numPoints = numPoints; - - if (computeAabb) - recalcLocalAabb(); - } - - SIMD_FORCE_INLINE btVector3* getUnscaledPoints() - { - return m_unscaledPoints; - } - - SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const - { - return m_unscaledPoints; - } - - SIMD_FORCE_INLINE int getNumPoints() const - { - return m_numPoints; - } - - SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const - { - return m_unscaledPoints[index] * m_localScaling; - } - -#ifndef __SPU__ - virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; -#endif - - - //debugging - virtual const char* getName()const {return "ConvexPointCloud";} - - virtual int getNumVertices() const; - virtual int getNumEdges() const; - virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; - virtual void getVertex(int i,btVector3& vtx) const; - virtual int getNumPlanes() const; - virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; - virtual bool isInside(const btVector3& pt,btScalar tolerance) const; - - ///in case we receive negative scaling - virtual void setLocalScaling(const btVector3& scaling); -}; - - -#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H +#define BT_CONVEX_POINT_CLOUD_SHAPE_H + +#include "btPolyhedralConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "LinearMath/btAlignedObjectArray.h" + +///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. +ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape +{ + btVector3* m_unscaledPoints; + int m_numPoints; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) + { + m_localScaling = localScaling; + m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + void setPoints (btVector3* points, int numPoints, bool computeAabb = true) + { + m_unscaledPoints = points; + m_numPoints = numPoints; + + if (computeAabb) + recalcLocalAabb(); + } + + SIMD_FORCE_INLINE btVector3* getUnscaledPoints() + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const + { + return m_unscaledPoints; + } + + SIMD_FORCE_INLINE int getNumPoints() const + { + return m_numPoints; + } + + SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const + { + return m_unscaledPoints[index] * m_localScaling; + } + +#ifndef __SPU__ + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; +#endif + + + //debugging + virtual const char* getName()const {return "ConvexPointCloud";} + + virtual int getNumVertices() const; + virtual int getNumEdges() const; + virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; + virtual void getVertex(int i,btVector3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; + virtual bool isInside(const btVector3& pt,btScalar tolerance) const; + + ///in case we receive negative scaling + virtual void setLocalScaling(const btVector3& scaling); +}; + + +#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H + diff --git a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp index 3b4fa4667..4bd986bb7 100644 --- a/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -1,314 +1,314 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btCollisionMargin.h" - -#include "LinearMath/btQuaternion.h" -#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" - - -btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) -: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) -{ - m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; - if ( calcAabb ) - recalcLocalAabb(); -} - - - - -///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once -///but then we are duplicating -class LocalSupportVertexCallback: public btInternalTriangleIndexCallback -{ - - btVector3 m_supportVertexLocal; -public: - - btScalar m_maxDot; - btVector3 m_supportVecLocal; - - LocalSupportVertexCallback(const btVector3& supportVecLocal) - : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), - m_maxDot(btScalar(-1e30)), - m_supportVecLocal(supportVecLocal) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - (void)triangleIndex; - (void)partId; - - for (int i=0;i<3;i++) - { - btScalar dot = m_supportVecLocal.dot(triangle[i]); - if (dot > m_maxDot) - { - m_maxDot = dot; - m_supportVertexLocal = triangle[i]; - } - } - } - - btVector3 GetSupportVertexLocal() - { - return m_supportVertexLocal; - } - -}; - - - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const -{ - btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - LocalSupportVertexCallback supportCallback(vec); - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); - supVec = supportCallback.GetSupportVertexLocal(); - - return supVec; -} - -void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const -{ - //use 'w' component of supportVerticesOut? - { - for (int i=0;iInternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); - supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); - } - -} - - - -btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const -{ - btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); - - if ( getMargin()!=btScalar(0.) ) - { - btVector3 vecnorm = vec; - if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) - { - vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); - } - vecnorm.normalize(); - supVertex+= getMargin() * vecnorm; - } - return supVertex; -} - - - - - - - - - -//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection -//Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo -int btConvexTriangleMeshShape::getNumVertices() const -{ - //cache this? - return 0; - -} - -int btConvexTriangleMeshShape::getNumEdges() const -{ - return 0; -} - -void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const -{ - btAssert(0); -} - -void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const -{ - btAssert(0); -} - -int btConvexTriangleMeshShape::getNumPlanes() const -{ - return 0; -} - -void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const -{ - btAssert(0); -} - -//not yet -bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const -{ - btAssert(0); - return false; -} - - - -void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - m_stridingMesh->setScaling(scaling); - - recalcLocalAabb(); - -} - - -const btVector3& btConvexTriangleMeshShape::getLocalScaling() const -{ - return m_stridingMesh->getScaling(); -} - -void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const -{ - class CenterCallback: public btInternalTriangleIndexCallback - { - bool first; - btVector3 ref; - btVector3 sum; - btScalar volume; - - public: - - CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - if (first) - { - ref = triangle[0]; - first = false; - } - else - { - btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); - sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); - volume += vol; - } - } - - btVector3 getCenter() - { - return (volume > 0) ? sum / volume : ref; - } - - btScalar getVolume() - { - return volume * btScalar(1. / 6); - } - - }; - - class InertiaCallback: public btInternalTriangleIndexCallback - { - btMatrix3x3 sum; - btVector3 center; - - public: - - InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) - { - (void) triangleIndex; - (void) partId; - btMatrix3x3 i; - btVector3 a = triangle[0] - center; - btVector3 b = triangle[1] - center; - btVector3 c = triangle[2] - center; - btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); - for (int j = 0; j < 3; j++) - { - for (int k = 0; k <= j; k++) - { - i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) - + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); - } - } - btScalar i00 = -i[0][0]; - btScalar i11 = -i[1][1]; - btScalar i22 = -i[2][2]; - i[0][0] = i11 + i22; - i[1][1] = i22 + i00; - i[2][2] = i00 + i11; - sum[0] += i[0]; - sum[1] += i[1]; - sum[2] += i[2]; - } - - btMatrix3x3& getInertia() - { - return sum; - } - - }; - - CenterCallback centerCallback; - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); - btVector3 center = centerCallback.getCenter(); - principal.setOrigin(center); - volume = centerCallback.getVolume(); - - InertiaCallback inertiaCallback(center); - m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); - - btMatrix3x3& i = inertiaCallback.getInertia(); - i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); - inertia.setValue(i[0][0], i[1][1], i[2][2]); - inertia /= volume; -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" +#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" + + +btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) +: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) +{ + m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; + if ( calcAabb ) + recalcLocalAabb(); +} + + + + +///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once +///but then we are duplicating +class LocalSupportVertexCallback: public btInternalTriangleIndexCallback +{ + + btVector3 m_supportVertexLocal; +public: + + btScalar m_maxDot; + btVector3 m_supportVecLocal; + + LocalSupportVertexCallback(const btVector3& supportVecLocal) + : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), + m_maxDot(btScalar(-1e30)), + m_supportVecLocal(supportVecLocal) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + (void)triangleIndex; + (void)partId; + + for (int i=0;i<3;i++) + { + btScalar dot = m_supportVecLocal.dot(triangle[i]); + if (dot > m_maxDot) + { + m_maxDot = dot; + m_supportVertexLocal = triangle[i]; + } + } + } + + btVector3 GetSupportVertexLocal() + { + return m_supportVertexLocal; + } + +}; + + + + + +btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + supVec = supportCallback.GetSupportVertexLocal(); + + return supVec; +} + +void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + //use 'w' component of supportVerticesOut? + { + for (int i=0;iInternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); + } + +} + + + +btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=btScalar(0.) ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; +} + + + + + + + + + +//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection +//Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo +int btConvexTriangleMeshShape::getNumVertices() const +{ + //cache this? + return 0; + +} + +int btConvexTriangleMeshShape::getNumEdges() const +{ + return 0; +} + +void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const +{ + btAssert(0); +} + +void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const +{ + btAssert(0); +} + +int btConvexTriangleMeshShape::getNumPlanes() const +{ + return 0; +} + +void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const +{ + btAssert(0); +} + +//not yet +bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const +{ + btAssert(0); + return false; +} + + + +void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_stridingMesh->setScaling(scaling); + + recalcLocalAabb(); + +} + + +const btVector3& btConvexTriangleMeshShape::getLocalScaling() const +{ + return m_stridingMesh->getScaling(); +} + +void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const +{ + class CenterCallback: public btInternalTriangleIndexCallback + { + bool first; + btVector3 ref; + btVector3 sum; + btScalar volume; + + public: + + CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + if (first) + { + ref = triangle[0]; + first = false; + } + else + { + btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); + sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref)); + volume += vol; + } + } + + btVector3 getCenter() + { + return (volume > 0) ? sum / volume : ref; + } + + btScalar getVolume() + { + return volume * btScalar(1. / 6); + } + + }; + + class InertiaCallback: public btInternalTriangleIndexCallback + { + btMatrix3x3 sum; + btVector3 center; + + public: + + InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) + { + (void) triangleIndex; + (void) partId; + btMatrix3x3 i; + btVector3 a = triangle[0] - center; + btVector3 b = triangle[1] - center; + btVector3 c = triangle[2] - center; + btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); + for (int j = 0; j < 3; j++) + { + for (int k = 0; k <= j; k++) + { + i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j])); + } + } + btScalar i00 = -i[0][0]; + btScalar i11 = -i[1][1]; + btScalar i22 = -i[2][2]; + i[0][0] = i11 + i22; + i[1][1] = i22 + i00; + i[2][2] = i00 + i11; + sum[0] += i[0]; + sum[1] += i[1]; + sum[2] += i[2]; + } + + btMatrix3x3& getInertia() + { + return sum; + } + + }; + + CenterCallback centerCallback; + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + m_stridingMesh->InternalProcessAllTriangles(¢erCallback, -aabbMax, aabbMax); + btVector3 center = centerCallback.getCenter(); + principal.setOrigin(center); + volume = centerCallback.getVolume(); + + InertiaCallback inertiaCallback(center); + m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); + + btMatrix3x3& i = inertiaCallback.getInertia(); + i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); + inertia.setValue(i[0][0], i[1][1], i[2][2]); + inertia /= volume; +} + diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 875d58ef8..7d4875d80 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -1,411 +1,411 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btHeightfieldTerrainShape.h" - -#include "LinearMath/btTransformUtil.h" - - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape -( -int heightStickWidth, int heightStickLength, void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) -{ - initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); -} - - - -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) -{ - // legacy constructor: support only float or unsigned char, - // and min height is zero - PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; - btScalar minHeight = 0.0; - - // previously, height = uchar * maxHeight / 65535. - // So to preserve legacy behavior, heightScale = maxHeight / 65535 - btScalar heightScale = maxHeight / 65535; - - initialize(heightStickWidth, heightStickLength, heightfieldData, - heightScale, minHeight, maxHeight, upAxis, hdt, - flipQuadEdges); -} - - - -void btHeightfieldTerrainShape::initialize -( -int heightStickWidth, int heightStickLength, void* heightfieldData, -btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, -PHY_ScalarType hdt, bool flipQuadEdges -) -{ - // validation - btAssert(heightStickWidth > 1 && "bad width"); - btAssert(heightStickLength > 1 && "bad length"); - btAssert(heightfieldData && "null heightfield data"); - // btAssert(heightScale) -- do we care? Trust caller here - btAssert(minHeight <= maxHeight && "bad min/max height"); - btAssert(upAxis >= 0 && upAxis < 3 && - "bad upAxis--should be in range [0,2]"); - btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && - "Bad height data type enum"); - - // initialize member variables - m_shapeType = TERRAIN_SHAPE_PROXYTYPE; - m_heightStickWidth = heightStickWidth; - m_heightStickLength = heightStickLength; - m_minHeight = minHeight; - m_maxHeight = maxHeight; - m_width = (btScalar) (heightStickWidth - 1); - m_length = (btScalar) (heightStickLength - 1); - m_heightScale = heightScale; - m_heightfieldDataUnknown = heightfieldData; - m_heightDataType = hdt; - m_flipQuadEdges = flipQuadEdges; - m_useDiamondSubdivision = false; - m_upAxis = upAxis; - m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); - - // determine min/max axis-aligned bounding box (aabb) values - switch (m_upAxis) - { - case 0: - { - m_localAabbMin.setValue(m_minHeight, 0, 0); - m_localAabbMax.setValue(m_maxHeight, m_width, m_length); - break; - } - case 1: - { - m_localAabbMin.setValue(0, m_minHeight, 0); - m_localAabbMax.setValue(m_width, m_maxHeight, m_length); - break; - }; - case 2: - { - m_localAabbMin.setValue(0, 0, m_minHeight); - m_localAabbMax.setValue(m_width, m_length, m_maxHeight); - break; - } - default: - { - //need to get valid m_upAxis - btAssert(0 && "Bad m_upAxis"); - } - } - - // remember origin (defined as exact middle of aabb) - m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); -} - - - -btHeightfieldTerrainShape::~btHeightfieldTerrainShape() -{ -} - - - -void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); - - btVector3 localOrigin(0, 0, 0); - localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); - localOrigin *= m_localScaling; - - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents), - abs_b[1].dot(halfExtents), - abs_b[2].dot(halfExtents)); - extent += btVector3(getMargin(),getMargin(),getMargin()); - - aabbMin = center - extent; - aabbMax = center + extent; -} - - -/// This returns the "raw" (user's initial) height, not the actual height. -/// The actual height needs to be adjusted to be relative to the center -/// of the heightfield's AABB. -btScalar -btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const -{ - btScalar val = 0.f; - switch (m_heightDataType) - { - case PHY_FLOAT: - { - val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; - break; - } - - case PHY_UCHAR: - { - unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; - val = heightFieldValue * m_heightScale; - break; - } - - case PHY_SHORT: - { - short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; - val = hfValue * m_heightScale; - break; - } - - default: - { - btAssert(!"Bad m_heightDataType"); - } - } - - return val; -} - - - - -/// this returns the vertex in bullet-local coordinates -void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const -{ - btAssert(x>=0); - btAssert(y>=0); - btAssert(xstartX) - startX = quantizedAabbMin[1]; - if (quantizedAabbMax[1]startJ) - startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) - startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]startJ) - startJ = quantizedAabbMin[2]; - if (quantizedAabbMax[2]startX) - startX = quantizedAabbMin[0]; - if (quantizedAabbMax[0]startJ) - startJ = quantizedAabbMin[1]; - if (quantizedAabbMax[1]processTriangle(vertices,x,j); - //second triangle - getVertex(x,j,vertices[0]); - getVertex(x+1,j+1,vertices[1]); - getVertex(x,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); - } else - { - //first triangle - getVertex(x,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j,vertices[2]); - callback->processTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); - } - } - } - - - -} - -void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const -{ - //moving concave objects not supported - - inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); -} - -void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} -const btVector3& btHeightfieldTerrainShape::getLocalScaling() const -{ - return m_localScaling; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btHeightfieldTerrainShape.h" + +#include "LinearMath/btTransformUtil.h" + + + +btHeightfieldTerrainShape::btHeightfieldTerrainShape +( +int heightStickWidth, int heightStickLength, void* heightfieldData, +btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, +PHY_ScalarType hdt, bool flipQuadEdges +) +{ + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); +} + + + +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +{ + // legacy constructor: support only float or unsigned char, + // and min height is zero + PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; + btScalar minHeight = 0.0; + + // previously, height = uchar * maxHeight / 65535. + // So to preserve legacy behavior, heightScale = maxHeight / 65535 + btScalar heightScale = maxHeight / 65535; + + initialize(heightStickWidth, heightStickLength, heightfieldData, + heightScale, minHeight, maxHeight, upAxis, hdt, + flipQuadEdges); +} + + + +void btHeightfieldTerrainShape::initialize +( +int heightStickWidth, int heightStickLength, void* heightfieldData, +btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, +PHY_ScalarType hdt, bool flipQuadEdges +) +{ + // validation + btAssert(heightStickWidth > 1 && "bad width"); + btAssert(heightStickLength > 1 && "bad length"); + btAssert(heightfieldData && "null heightfield data"); + // btAssert(heightScale) -- do we care? Trust caller here + btAssert(minHeight <= maxHeight && "bad min/max height"); + btAssert(upAxis >= 0 && upAxis < 3 && + "bad upAxis--should be in range [0,2]"); + btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && + "Bad height data type enum"); + + // initialize member variables + m_shapeType = TERRAIN_SHAPE_PROXYTYPE; + m_heightStickWidth = heightStickWidth; + m_heightStickLength = heightStickLength; + m_minHeight = minHeight; + m_maxHeight = maxHeight; + m_width = (btScalar) (heightStickWidth - 1); + m_length = (btScalar) (heightStickLength - 1); + m_heightScale = heightScale; + m_heightfieldDataUnknown = heightfieldData; + m_heightDataType = hdt; + m_flipQuadEdges = flipQuadEdges; + m_useDiamondSubdivision = false; + m_upAxis = upAxis; + m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); + + // determine min/max axis-aligned bounding box (aabb) values + switch (m_upAxis) + { + case 0: + { + m_localAabbMin.setValue(m_minHeight, 0, 0); + m_localAabbMax.setValue(m_maxHeight, m_width, m_length); + break; + } + case 1: + { + m_localAabbMin.setValue(0, m_minHeight, 0); + m_localAabbMax.setValue(m_width, m_maxHeight, m_length); + break; + }; + case 2: + { + m_localAabbMin.setValue(0, 0, m_minHeight); + m_localAabbMax.setValue(m_width, m_length, m_maxHeight); + break; + } + default: + { + //need to get valid m_upAxis + btAssert(0 && "Bad m_upAxis"); + } + } + + // remember origin (defined as exact middle of aabb) + m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); +} + + + +btHeightfieldTerrainShape::~btHeightfieldTerrainShape() +{ +} + + + +void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + + btVector3 localOrigin(0, 0, 0); + localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); + localOrigin *= m_localScaling; + + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents), + abs_b[1].dot(halfExtents), + abs_b[2].dot(halfExtents)); + extent += btVector3(getMargin(),getMargin(),getMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; +} + + +/// This returns the "raw" (user's initial) height, not the actual height. +/// The actual height needs to be adjusted to be relative to the center +/// of the heightfield's AABB. +btScalar +btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const +{ + btScalar val = 0.f; + switch (m_heightDataType) + { + case PHY_FLOAT: + { + val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; + break; + } + + case PHY_UCHAR: + { + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; + val = heightFieldValue * m_heightScale; + break; + } + + case PHY_SHORT: + { + short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; + val = hfValue * m_heightScale; + break; + } + + default: + { + btAssert(!"Bad m_heightDataType"); + } + } + + return val; +} + + + + +/// this returns the vertex in bullet-local coordinates +void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +{ + btAssert(x>=0); + btAssert(y>=0); + btAssert(xstartX) + startX = quantizedAabbMin[1]; + if (quantizedAabbMax[1]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[1]; + if (quantizedAabbMax[1]processTriangle(vertices,x,j); + //second triangle + getVertex(x,j,vertices[0]); + getVertex(x+1,j+1,vertices[1]); + getVertex(x,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + } else + { + //first triangle + getVertex(x,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j,vertices[2]); + callback->processTriangle(vertices,x,j); + //second triangle + getVertex(x+1,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + } + } + } + + + +} + +void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const +{ + //moving concave objects not supported + + inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); +} + +void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} +const btVector3& btHeightfieldTerrainShape::getLocalScaling() const +{ + return m_localScaling; +} diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 41f8a0374..36489a0eb 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -1,161 +1,161 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H -#define HEIGHTFIELD_TERRAIN_SHAPE_H - -#include "btConcaveShape.h" - -///btHeightfieldTerrainShape simulates a 2D heightfield terrain -/** - The caller is responsible for maintaining the heightfield array; this - class does not make a copy. - - The heightfield can be dynamic so long as the min/max height values - capture the extremes (heights must always be in that range). - - The local origin of the heightfield is assumed to be the exact - center (as determined by width and length and height, with each - axis multiplied by the localScaling). - - \b NOTE: be careful with coordinates. If you have a heightfield with a local - min height of -100m, and a max height of +500m, you may be tempted to place it - at the origin (0,0) and expect the heights in world coordinates to be - -100 to +500 meters. - Actually, the heights will be -300 to +300m, because bullet will re-center - the heightfield based on its AABB (which is determined by the min/max - heights). So keep in mind that once you create a btHeightfieldTerrainShape - object, the heights will be adjusted relative to the center of the AABB. This - is different to the behavior of many rendering engines, but is useful for - physics engines. - - Most (but not all) rendering and heightfield libraries assume upAxis = 1 - (that is, the y-axis is "up"). This class allows any of the 3 coordinates - to be "up". Make sure your choice of axis is consistent with your rendering - system. - - The heightfield heights are determined from the data type used for the - heightfieldData array. - - - PHY_UCHAR: height at a point is the uchar value at the - grid point, multipled by heightScale. uchar isn't recommended - because of its inability to deal with negative values, and - low resolution (8-bit). - - - PHY_SHORT: height at a point is the short int value at that grid - point, multipled by heightScale. - - - PHY_FLOAT: height at a point is the float value at that grid - point. heightScale is ignored when using the float heightfield - data type. - - Whatever the caller specifies as minHeight and maxHeight will be honored. - The class will not inspect the heightfield to discover the actual minimum - or maximum heights. These values are used to determine the heightfield's - axis-aligned bounding box, multiplied by localScaling. - - For usage and testing see the TerrainDemo. - */ -class btHeightfieldTerrainShape : public btConcaveShape -{ -protected: - btVector3 m_localAabbMin; - btVector3 m_localAabbMax; - btVector3 m_localOrigin; - - ///terrain data - int m_heightStickWidth; - int m_heightStickLength; - btScalar m_minHeight; - btScalar m_maxHeight; - btScalar m_width; - btScalar m_length; - btScalar m_heightScale; - union - { - unsigned char* m_heightfieldDataUnsignedChar; - short* m_heightfieldDataShort; - btScalar* m_heightfieldDataFloat; - void* m_heightfieldDataUnknown; - }; - - PHY_ScalarType m_heightDataType; - bool m_flipQuadEdges; - bool m_useDiamondSubdivision; - - int m_upAxis; - - btVector3 m_localScaling; - - virtual btScalar getRawHeightFieldValue(int x,int y) const; - void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; - void getVertex(int x,int y,btVector3& vertex) const; - - - - /// protected initialization - /** - Handles the work of constructors so that public constructors can be - backwards-compatible without a lot of copy/paste. - */ - void initialize(int heightStickWidth, int heightStickLength, - void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, int upAxis, - PHY_ScalarType heightDataType, bool flipQuadEdges); - -public: - /// preferred constructor - /** - This constructor supports a range of heightfield - data types, and allows for a non-zero minimum height value. - heightScale is needed for any integer-based heightfield data types. - */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, - void* heightfieldData, btScalar heightScale, - btScalar minHeight, btScalar maxHeight, - int upAxis, PHY_ScalarType heightDataType, - bool flipQuadEdges); - - /// legacy constructor - /** - The legacy constructor assumes the heightfield has a minimum height - of zero. Only unsigned char or floats are supported. For legacy - compatibility reasons, heightScale is calculated as maxHeight / 65535 - (and is only used when useFloatData = false). - */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); - - virtual ~btHeightfieldTerrainShape(); - - - void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} - - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void setLocalScaling(const btVector3& scaling); - - virtual const btVector3& getLocalScaling() const; - - //debugging - virtual const char* getName()const {return "HEIGHTFIELD";} - -}; - -#endif //HEIGHTFIELD_TERRAIN_SHAPE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H +#define HEIGHTFIELD_TERRAIN_SHAPE_H + +#include "btConcaveShape.h" + +///btHeightfieldTerrainShape simulates a 2D heightfield terrain +/** + The caller is responsible for maintaining the heightfield array; this + class does not make a copy. + + The heightfield can be dynamic so long as the min/max height values + capture the extremes (heights must always be in that range). + + The local origin of the heightfield is assumed to be the exact + center (as determined by width and length and height, with each + axis multiplied by the localScaling). + + \b NOTE: be careful with coordinates. If you have a heightfield with a local + min height of -100m, and a max height of +500m, you may be tempted to place it + at the origin (0,0) and expect the heights in world coordinates to be + -100 to +500 meters. + Actually, the heights will be -300 to +300m, because bullet will re-center + the heightfield based on its AABB (which is determined by the min/max + heights). So keep in mind that once you create a btHeightfieldTerrainShape + object, the heights will be adjusted relative to the center of the AABB. This + is different to the behavior of many rendering engines, but is useful for + physics engines. + + Most (but not all) rendering and heightfield libraries assume upAxis = 1 + (that is, the y-axis is "up"). This class allows any of the 3 coordinates + to be "up". Make sure your choice of axis is consistent with your rendering + system. + + The heightfield heights are determined from the data type used for the + heightfieldData array. + + - PHY_UCHAR: height at a point is the uchar value at the + grid point, multipled by heightScale. uchar isn't recommended + because of its inability to deal with negative values, and + low resolution (8-bit). + + - PHY_SHORT: height at a point is the short int value at that grid + point, multipled by heightScale. + + - PHY_FLOAT: height at a point is the float value at that grid + point. heightScale is ignored when using the float heightfield + data type. + + Whatever the caller specifies as minHeight and maxHeight will be honored. + The class will not inspect the heightfield to discover the actual minimum + or maximum heights. These values are used to determine the heightfield's + axis-aligned bounding box, multiplied by localScaling. + + For usage and testing see the TerrainDemo. + */ +class btHeightfieldTerrainShape : public btConcaveShape +{ +protected: + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + btVector3 m_localOrigin; + + ///terrain data + int m_heightStickWidth; + int m_heightStickLength; + btScalar m_minHeight; + btScalar m_maxHeight; + btScalar m_width; + btScalar m_length; + btScalar m_heightScale; + union + { + unsigned char* m_heightfieldDataUnsignedChar; + short* m_heightfieldDataShort; + btScalar* m_heightfieldDataFloat; + void* m_heightfieldDataUnknown; + }; + + PHY_ScalarType m_heightDataType; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; + + int m_upAxis; + + btVector3 m_localScaling; + + virtual btScalar getRawHeightFieldValue(int x,int y) const; + void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; + void getVertex(int x,int y,btVector3& vertex) const; + + + + /// protected initialization + /** + Handles the work of constructors so that public constructors can be + backwards-compatible without a lot of copy/paste. + */ + void initialize(int heightStickWidth, int heightStickLength, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, int upAxis, + PHY_ScalarType heightDataType, bool flipQuadEdges); + +public: + /// preferred constructor + /** + This constructor supports a range of heightfield + data types, and allows for a non-zero minimum height value. + heightScale is needed for any integer-based heightfield data types. + */ + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, + void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, PHY_ScalarType heightDataType, + bool flipQuadEdges); + + /// legacy constructor + /** + The legacy constructor assumes the heightfield has a minimum height + of zero. Only unsigned char or floats are supported. For legacy + compatibility reasons, heightScale is calculated as maxHeight / 65535 + (and is only used when useFloatData = false). + */ + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + + virtual ~btHeightfieldTerrainShape(); + + + void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void setLocalScaling(const btVector3& scaling); + + virtual const btVector3& getLocalScaling() const; + + //debugging + virtual const char* getName()const {return "HEIGHTFIELD";} + +}; + +#endif //HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btMaterial.h b/src/BulletCollision/CollisionShapes/btMaterial.h index 751e58112..7cb6d5ab6 100644 --- a/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/src/BulletCollision/CollisionShapes/btMaterial.h @@ -1,34 +1,34 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#ifndef MATERIAL_H -#define MATERIAL_H - -// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties -class btMaterial -{ - // public members so that materials can change due to world events -public: - btScalar m_friction; - btScalar m_restitution; - int pad[2]; - - btMaterial(){} - btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } -}; - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#ifndef MATERIAL_H +#define MATERIAL_H + +// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties +class btMaterial +{ + // public members so that materials can change due to world events +public: + btScalar m_friction; + btScalar m_restitution; + int pad[2]; + + btMaterial(){} + btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } +}; + #endif // MATERIAL_H \ No newline at end of file diff --git a/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp index 54501e6c3..fc47e8641 100644 --- a/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp @@ -1,45 +1,45 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" -//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - - -///Obtains the material for a specific triangle -const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) -{ - const unsigned char * materialBase = 0; - int numMaterials; - PHY_ScalarType materialType; - int materialStride; - const unsigned char * triangleMaterialBase = 0; - int numTriangles; - int triangleMaterialStride; - PHY_ScalarType triangleType; - - ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, - &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); - - // return the pointer to the place with the friction for the triangle - // TODO: This depends on whether it's a moving mesh or not - // BUG IN GIMPACT - //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); - int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); - btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); - return (matVal); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" +//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + + +///Obtains the material for a specific triangle +const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) +{ + const unsigned char * materialBase = 0; + int numMaterials; + PHY_ScalarType materialType; + int materialStride; + const unsigned char * triangleMaterialBase = 0; + int numTriangles; + int triangleMaterialStride; + PHY_ScalarType triangleType; + + ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, + &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); + + // return the pointer to the place with the friction for the triangle + // TODO: This depends on whether it's a moving mesh or not + // BUG IN GIMPACT + //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); + int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); + btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); + return (matVal); +} diff --git a/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 86dd81a01..d025a5c44 100644 --- a/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -1,123 +1,123 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// This file was created by Alex Silverman - -#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H -#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H - -#include "btBvhTriangleMeshShape.h" -#include "btMaterial.h" - -///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. -ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape -{ - btAlignedObjectArray m_materialList; - int ** m_triangleMaterials; - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;} - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - btVector3 m_triangle[3]; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); - } - } - - ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb - btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): - btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) - { - m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; - - btVector3 m_triangle[3]; - const unsigned char *vertexbase; - int numverts; - PHY_ScalarType type; - int stride; - const unsigned char *indexbase; - int indexstride; - int numfaces; - PHY_ScalarType indicestype; - - //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); - - for(int i = 0; i < meshInterface->getNumSubParts(); i++) - { - m_meshInterface->getLockedReadOnlyVertexIndexBase( - &vertexbase, - numverts, - type, - stride, - &indexbase, - indexstride, - numfaces, - indicestype, - i); - //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); - } - } - - virtual ~btMultimaterialTriangleMeshShape() - { -/* - for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) - { - btAlignedFree(m_materialValues[i]); - m_materialLookup[i] = NULL; - } - btAlignedFree(m_materialValues); - m_materialLookup = NULL; -*/ - } - //debugging - virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} - - ///Obtains the material for a specific triangle - const btMaterial * getMaterialProperties(int partID, int triIndex); - -} -; - -#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/// This file was created by Alex Silverman + +#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H + +#include "btBvhTriangleMeshShape.h" +#include "btMaterial.h" + +///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution. +ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape +{ + btAlignedObjectArray m_materialList; + int ** m_triangleMaterials; + +public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;} + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); + } + } + + ///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb + btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true): + btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) + { + m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; + + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); + + for(int i = 0; i < meshInterface->getNumSubParts(); i++) + { + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + i); + //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); + } + } + + virtual ~btMultimaterialTriangleMeshShape() + { +/* + for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) + { + btAlignedFree(m_materialValues[i]); + m_materialLookup[i] = NULL; + } + btAlignedFree(m_materialValues); + m_materialLookup = NULL; +*/ + } + //debugging + virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} + + ///Obtains the material for a specific triangle + const btMaterial * getMaterialProperties(int partID, int triIndex); + +} +; + +#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp index c1ec6641b..1fac15387 100644 --- a/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -1,120 +1,120 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btScaledBvhTriangleMeshShape.h" - -btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) -:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) -{ - m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; -} - -btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() -{ -} - - -class btScaledTriangleCallback : public btTriangleCallback -{ - btTriangleCallback* m_originalCallback; - - btVector3 m_localScaling; - -public: - - btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) - :m_originalCallback(originalCallback), - m_localScaling(localScaling) - { - } - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - btVector3 newTriangle[3]; - newTriangle[0] = triangle[0]*m_localScaling; - newTriangle[1] = triangle[1]*m_localScaling; - newTriangle[2] = triangle[2]*m_localScaling; - m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); - } -}; - -void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const -{ - btScaledTriangleCallback scaledCallback(callback,m_localScaling); - - btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); - btVector3 scaledAabbMin,scaledAabbMax; - - ///support negative scaling - scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; - scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; - scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; - - scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; - scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; - scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; - - - m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); -} - - -void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const -{ - btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); - btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); - - btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; - btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; - - localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; - localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; - localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; - localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; - localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; - - btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); - btScalar margin = m_bvhTriMeshShape->getMargin(); - localHalfExtents += btVector3(margin,margin,margin); - btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); - - btMatrix3x3 abs_b = trans.getBasis().absolute(); - - btVector3 center = trans(localCenter); - - btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), - abs_b[1].dot(localHalfExtents), - abs_b[2].dot(localHalfExtents)); - aabbMin = center - extent; - aabbMax = center + extent; - -} - -void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) -{ - m_localScaling = scaling; -} - -const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const -{ - return m_localScaling; -} - -void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const -{ - ///don't make this a movable object! -// btAssert(0); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btScaledBvhTriangleMeshShape.h" + +btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) +:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) +{ + m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; +} + +btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() +{ +} + + +class btScaledTriangleCallback : public btTriangleCallback +{ + btTriangleCallback* m_originalCallback; + + btVector3 m_localScaling; + +public: + + btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) + :m_originalCallback(originalCallback), + m_localScaling(localScaling) + { + } + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + btVector3 newTriangle[3]; + newTriangle[0] = triangle[0]*m_localScaling; + newTriangle[1] = triangle[1]*m_localScaling; + newTriangle[2] = triangle[2]*m_localScaling; + m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); + } +}; + +void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + btScaledTriangleCallback scaledCallback(callback,m_localScaling); + + btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); + btVector3 scaledAabbMin,scaledAabbMax; + + ///support negative scaling + scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMin[1] = m_localScaling.getY() >= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMin[2] = m_localScaling.getZ() >= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + scaledAabbMax[0] = m_localScaling.getX() <= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; + scaledAabbMax[1] = m_localScaling.getY() <= 0. ? aabbMin[1] * invLocalScaling[1] : aabbMax[1] * invLocalScaling[1]; + scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2]; + + + m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); +} + + +void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); + btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); + + btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; + btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; + + localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; + localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; + localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; + + btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); + btScalar margin = m_bvhTriMeshShape->getMargin(); + localHalfExtents += btVector3(margin,margin,margin); + btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btVector3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + aabbMin = center - extent; + aabbMax = center + extent; + +} + +void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} + +const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const +{ + return m_localScaling; +} + +void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///don't make this a movable object! +// btAssert(0); +} diff --git a/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 8deeac90f..5da6fe8bc 100644 --- a/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -1,62 +1,62 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H -#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H - -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" - - -///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. -///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' -ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape -{ - - - btVector3 m_localScaling; - - btBvhTriangleMeshShape* m_bvhTriMeshShape; - -public: - - - btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); - - virtual ~btScaledBvhTriangleMeshShape(); - - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; - virtual void setLocalScaling(const btVector3& scaling); - virtual const btVector3& getLocalScaling() const; - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; - - virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; - - btBvhTriangleMeshShape* getChildShape() - { - return m_bvhTriMeshShape; - } - - const btBvhTriangleMeshShape* getChildShape() const - { - return m_bvhTriMeshShape; - } - - //debugging - virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} - -}; - -#endif //BVH_TRIANGLE_MESH_SHAPE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + +///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. +///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling' +ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape +{ + + + btVector3 m_localScaling; + + btBvhTriangleMeshShape* m_bvhTriMeshShape; + +public: + + + btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); + + virtual ~btScaledBvhTriangleMeshShape(); + + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + btBvhTriangleMeshShape* getChildShape() + { + return m_bvhTriMeshShape; + } + + const btBvhTriangleMeshShape* getChildShape() const + { + return m_bvhTriMeshShape; + } + + //debugging + virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp index 93ff550c8..a87b87f1a 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -1,164 +1,164 @@ -/* -btbtShapeHull implemented by John McCutchan. - -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btShapeHull.h" -#include "LinearMath/btConvexHull.h" - -#define NUM_UNITSPHERE_POINTS 42 - -static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = -{ - btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), - btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), - btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), - btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), - btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), - btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), - btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), - btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), - btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), - btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), - btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), - btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), - btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), - btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), - btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), - btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), - btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), - btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), - btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), - btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), - btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), - btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), - btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), - btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), - btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), - btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), - btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), - btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), - btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), - btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), - btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), - btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), - btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), - btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), - btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), - btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) -}; - -btShapeHull::btShapeHull (const btConvexShape* shape) -{ - m_shape = shape; - m_vertices.clear (); - m_indices.clear(); - m_numIndices = 0; -} - -btShapeHull::~btShapeHull () -{ - m_indices.clear(); - m_vertices.clear (); -} - -bool -btShapeHull::buildHull (btScalar /*margin*/) -{ - int numSampleDirections = NUM_UNITSPHERE_POINTS; - { - int numPDA = m_shape->getNumPreferredPenetrationDirections(); - if (numPDA) - { - for (int i=0;igetPreferredPenetrationDirection(i,norm); - btUnitSpherePoints[numSampleDirections] = norm; - numSampleDirections++; - } - } - } - - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; - int i; - for (i = 0; i < numSampleDirections; i++) - { - supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); - } - - HullDesc hd; - hd.mFlags = QF_TRIANGLES; - hd.mVcount = static_cast(numSampleDirections); - -#ifdef BT_USE_DOUBLE_PRECISION - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof(btVector3); -#else - hd.mVertices = &supportPoints[0]; - hd.mVertexStride = sizeof (btVector3); -#endif - - HullLibrary hl; - HullResult hr; - if (hl.CreateConvexHull (hd, hr) == QE_FAIL) - { - return false; - } - - m_vertices.resize (static_cast(hr.mNumOutputVertices)); - - - for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) - { - m_vertices[i] = hr.m_OutputVertices[i]; - } - m_numIndices = hr.mNumIndices; - m_indices.resize(static_cast(m_numIndices)); - for (i = 0; i < static_cast(m_numIndices); i++) - { - m_indices[i] = hr.m_Indices[i]; - } - - // free temporary hull result that we just copied - hl.ReleaseResult (hr); - - return true; -} - -int -btShapeHull::numTriangles () const -{ - return static_cast(m_numIndices / 3); -} - -int -btShapeHull::numVertices () const -{ - return m_vertices.size (); -} - -int -btShapeHull::numIndices () const -{ - return static_cast(m_numIndices); -} - +/* +btbtShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btShapeHull.h" +#include "LinearMath/btConvexHull.h" + +#define NUM_UNITSPHERE_POINTS 42 + +static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = +{ + btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), + btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), + btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)), + btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), + btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), + btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)), + btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)), + btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)), + btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)), + btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)), + btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), + btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), + btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)), + btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)), + btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), + btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), + btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), + btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), + btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), + btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), + btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), + btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)), + btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)), + btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)), + btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)), + btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)), + btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)), + btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)), + btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), + btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), + btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), + btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), + btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), + btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)), + btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), + btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) +}; + +btShapeHull::btShapeHull (const btConvexShape* shape) +{ + m_shape = shape; + m_vertices.clear (); + m_indices.clear(); + m_numIndices = 0; +} + +btShapeHull::~btShapeHull () +{ + m_indices.clear(); + m_vertices.clear (); +} + +bool +btShapeHull::buildHull (btScalar /*margin*/) +{ + int numSampleDirections = NUM_UNITSPHERE_POINTS; + { + int numPDA = m_shape->getNumPreferredPenetrationDirections(); + if (numPDA) + { + for (int i=0;igetPreferredPenetrationDirection(i,norm); + btUnitSpherePoints[numSampleDirections] = norm; + numSampleDirections++; + } + } + } + + btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + int i; + for (i = 0; i < numSampleDirections; i++) + { + supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); + } + + HullDesc hd; + hd.mFlags = QF_TRIANGLES; + hd.mVcount = static_cast(numSampleDirections); + +#ifdef BT_USE_DOUBLE_PRECISION + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof(btVector3); +#else + hd.mVertices = &supportPoints[0]; + hd.mVertexStride = sizeof (btVector3); +#endif + + HullLibrary hl; + HullResult hr; + if (hl.CreateConvexHull (hd, hr) == QE_FAIL) + { + return false; + } + + m_vertices.resize (static_cast(hr.mNumOutputVertices)); + + + for (i = 0; i < static_cast(hr.mNumOutputVertices); i++) + { + m_vertices[i] = hr.m_OutputVertices[i]; + } + m_numIndices = hr.mNumIndices; + m_indices.resize(static_cast(m_numIndices)); + for (i = 0; i < static_cast(m_numIndices); i++) + { + m_indices[i] = hr.m_Indices[i]; + } + + // free temporary hull result that we just copied + hl.ReleaseResult (hr); + + return true; +} + +int +btShapeHull::numTriangles () const +{ + return static_cast(m_numIndices / 3); +} + +int +btShapeHull::numVertices () const +{ + return m_vertices.size (); +} + +int +btShapeHull::numIndices () const +{ + return static_cast(m_numIndices); +} + diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.h b/src/BulletCollision/CollisionShapes/btShapeHull.h index 8fce57b7c..583c6b99e 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -1,56 +1,56 @@ -/* -btShapeHull implemented by John McCutchan. - -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef _SHAPE_HULL_H -#define _SHAPE_HULL_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "BulletCollision/CollisionShapes/btConvexShape.h" - - -///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. -///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. -///It approximates the convex hull using the supporting vertex of 42 directions. -class btShapeHull -{ -public: - btShapeHull (const btConvexShape* shape); - ~btShapeHull (); - - bool buildHull (btScalar margin); - - int numTriangles () const; - int numVertices () const; - int numIndices () const; - - const btVector3* getVertexPointer() const - { - return &m_vertices[0]; - } - const unsigned int* getIndexPointer() const - { - return &m_indices[0]; - } - -protected: - btAlignedObjectArray m_vertices; - btAlignedObjectArray m_indices; - unsigned int m_numIndices; - const btConvexShape* m_shape; -}; - -#endif //_SHAPE_HULL_H +/* +btShapeHull implemented by John McCutchan. + +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef _SHAPE_HULL_H +#define _SHAPE_HULL_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + + +///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices. +///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object. +///It approximates the convex hull using the supporting vertex of 42 directions. +class btShapeHull +{ +public: + btShapeHull (const btConvexShape* shape); + ~btShapeHull (); + + bool buildHull (btScalar margin); + + int numTriangles () const; + int numVertices () const; + int numIndices () const; + + const btVector3* getVertexPointer() const + { + return &m_vertices[0]; + } + const unsigned int* getIndexPointer() const + { + return &m_indices[0]; + } + +protected: + btAlignedObjectArray m_vertices; + btAlignedObjectArray m_indices; + unsigned int m_numIndices; + const btConvexShape* m_shape; +}; + +#endif //_SHAPE_HULL_H diff --git a/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp b/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp index c84180442..5194219f2 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp +++ b/src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp @@ -1,35 +1,35 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btTriangleBuffer.h" - - - - - - - -void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) -{ - btTriangle tri; - tri.m_vertex0 = triangle[0]; - tri.m_vertex1 = triangle[1]; - tri.m_vertex2 = triangle[2]; - tri.m_partId = partId; - tri.m_triangleIndex = triangleIndex; - - m_triangleBuffer.push_back(tri); -} - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btTriangleBuffer.h" + + + + + + + +void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) +{ + btTriangle tri; + tri.m_vertex0 = triangle[0]; + tri.m_vertex1 = triangle[1]; + tri.m_vertex2 = triangle[2]; + tri.m_partId = partId; + tri.m_triangleIndex = triangleIndex; + + m_triangleBuffer.push_back(tri); +} + diff --git a/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index 002d1a3a8..adca38041 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -1,69 +1,69 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_TRIANGLE_BUFFER_H -#define BT_TRIANGLE_BUFFER_H - -#include "btTriangleCallback.h" -#include "LinearMath/btAlignedObjectArray.h" - -struct btTriangle -{ - btVector3 m_vertex0; - btVector3 m_vertex1; - btVector3 m_vertex2; - int m_partId; - int m_triangleIndex; -}; - -///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' -///Example usage of this class: -/// btTriangleBuffer triBuf; -/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); -/// for (int i=0;i m_triangleBuffer; - -public: - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - int getNumTriangles() const - { - return int(m_triangleBuffer.size()); - } - - const btTriangle& getTriangle(int index) const - { - return m_triangleBuffer[index]; - } - - void clearBuffer() - { - m_triangleBuffer.clear(); - } - -}; - - -#endif //BT_TRIANGLE_BUFFER_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_TRIANGLE_BUFFER_H +#define BT_TRIANGLE_BUFFER_H + +#include "btTriangleCallback.h" +#include "LinearMath/btAlignedObjectArray.h" + +struct btTriangle +{ + btVector3 m_vertex0; + btVector3 m_vertex1; + btVector3 m_vertex2; + int m_partId; + int m_triangleIndex; +}; + +///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' +///Example usage of this class: +/// btTriangleBuffer triBuf; +/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); +/// for (int i=0;i m_triangleBuffer; + +public: + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + int getNumTriangles() const + { + return int(m_triangleBuffer.size()); + } + + const btTriangle& getTriangle(int index) const + { + return m_triangleBuffer[index]; + } + + void clearBuffer() + { + m_triangleBuffer.clear(); + } + +}; + + +#endif //BT_TRIANGLE_BUFFER_H + diff --git a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp index bf02039b9..5f2925328 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp +++ b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -82,6 +82,7 @@ bool btTriangleIndexVertexArray::hasPremadeAabb() const return (m_hasAabb == 1); } + void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const { m_aabbMin = aabbMin; diff --git a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp index 19f0bde12..abefa7125 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp +++ b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp @@ -1,86 +1,87 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was created by Alex Silverman - -#include "btTriangleIndexVertexMaterialArray.h" - -btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride) : -btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) -{ - btMaterialProperties mat; - - mat.m_numMaterials = numMaterials; - mat.m_materialBase = materialBase; - mat.m_materialStride = materialStride; -#ifdef BT_USE_DOUBLE_PRECISION - mat.m_materialType = PHY_DOUBLE; -#else - mat.m_materialType = PHY_FLOAT; -#endif - - mat.m_numTriangles = numTriangles; - mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; - mat.m_triangleMaterialStride = materialIndexStride; - mat.m_triangleType = PHY_INTEGER; - - addMaterialProperties(mat); -} - - -void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) -{ - btAssert(subpart< getNumSubParts() ); - - btMaterialProperties& mats = m_materials[subpart]; - - numMaterials = mats.m_numMaterials; - (*materialBase) = (unsigned char *) mats.m_materialBase; -#ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; -#else - materialType = PHY_FLOAT; -#endif - materialStride = mats.m_materialStride; - - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; -} - -void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) -{ - btMaterialProperties& mats = m_materials[subpart]; - - numMaterials = mats.m_numMaterials; - (*materialBase) = (const unsigned char *) mats.m_materialBase; -#ifdef BT_USE_DOUBLE_PRECISION - materialType = PHY_DOUBLE; -#else - materialType = PHY_FLOAT; -#endif - materialStride = mats.m_materialStride; - - numTriangles = mats.m_numTriangles; - (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; - triangleMaterialStride = mats.m_triangleMaterialStride; - triangleType = mats.m_triangleType; -} + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was created by Alex Silverman + +#include "btTriangleIndexVertexMaterialArray.h" + +btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride) : +btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) +{ + btMaterialProperties mat; + + mat.m_numMaterials = numMaterials; + mat.m_materialBase = materialBase; + mat.m_materialStride = materialStride; +#ifdef BT_USE_DOUBLE_PRECISION + mat.m_materialType = PHY_DOUBLE; +#else + mat.m_materialType = PHY_FLOAT; +#endif + + mat.m_numTriangles = numTriangles; + mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; + mat.m_triangleMaterialStride = materialIndexStride; + mat.m_triangleType = PHY_INTEGER; + + addMaterialProperties(mat); +} + + +void btTriangleIndexVertexMaterialArray::getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btAssert(subpart< getNumSubParts() ); + + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} + +void btTriangleIndexVertexMaterialArray::getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart) +{ + btMaterialProperties& mats = m_materials[subpart]; + + numMaterials = mats.m_numMaterials; + (*materialBase) = (const unsigned char *) mats.m_materialBase; +#ifdef BT_USE_DOUBLE_PRECISION + materialType = PHY_DOUBLE; +#else + materialType = PHY_FLOAT; +#endif + materialStride = mats.m_materialStride; + + numTriangles = mats.m_numTriangles; + (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; + triangleMaterialStride = mats.m_triangleMaterialStride; + triangleType = mats.m_triangleType; +} diff --git a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h index 7c7cef8b7..69a2e6314 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h +++ b/src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h @@ -1,84 +1,84 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///This file was created by Alex Silverman - -#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H -#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H - -#include "btTriangleIndexVertexArray.h" - - -ATTRIBUTE_ALIGNED16( struct) btMaterialProperties -{ - ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution - int m_numMaterials; - const unsigned char * m_materialBase; - int m_materialStride; - PHY_ScalarType m_materialType; - ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're - /// padding the structure, it can be reproduced at no real cost - ///m_triangleMaterials =====> 1 integer value makes up one entry - /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 - int m_numTriangles; - const unsigned char * m_triangleMaterialsBase; - int m_triangleMaterialStride; - ///m_triangleType <========== Automatically set in addMaterialProperties - PHY_ScalarType m_triangleType; -}; - -typedef btAlignedObjectArray MaterialArray; - -///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray -///The addition of a material array allows for the utilization of the partID and -///triangleIndex that are returned in the ContactAddedCallback. As with -///TriangleIndexVertexArray, no duplicate is made of the material data, so it -///is the users responsibility to maintain the array during the lifetime of the -///TriangleIndexVertexMaterialArray. -ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray -{ -protected: - MaterialArray m_materials; - -public: - BT_DECLARE_ALIGNED_ALLOCATOR(); - - btTriangleIndexVertexMaterialArray() - { - } - - btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, - int numVertices,btScalar* vertexBase,int vertexStride, - int numMaterials, unsigned char* materialBase, int materialStride, - int* triangleMaterialsBase, int materialIndexStride); - - virtual ~btTriangleIndexVertexMaterialArray() {} - - void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) - { - m_materials.push_back(mat); - m_materials[m_materials.size()-1].m_triangleType = triangleType; - } - - virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); - - virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, - const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); - -} -; - -#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///This file was created by Alex Silverman + +#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H +#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H + +#include "btTriangleIndexVertexArray.h" + + +ATTRIBUTE_ALIGNED16( struct) btMaterialProperties +{ + ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution + int m_numMaterials; + const unsigned char * m_materialBase; + int m_materialStride; + PHY_ScalarType m_materialType; + ///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're + /// padding the structure, it can be reproduced at no real cost + ///m_triangleMaterials =====> 1 integer value makes up one entry + /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 + int m_numTriangles; + const unsigned char * m_triangleMaterialsBase; + int m_triangleMaterialStride; + ///m_triangleType <========== Automatically set in addMaterialProperties + PHY_ScalarType m_triangleType; +}; + +typedef btAlignedObjectArray MaterialArray; + +///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray +///The addition of a material array allows for the utilization of the partID and +///triangleIndex that are returned in the ContactAddedCallback. As with +///TriangleIndexVertexArray, no duplicate is made of the material data, so it +///is the users responsibility to maintain the array during the lifetime of the +///TriangleIndexVertexMaterialArray. +ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray +{ +protected: + MaterialArray m_materials; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btTriangleIndexVertexMaterialArray() + { + } + + btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, + int numVertices,btScalar* vertexBase,int vertexStride, + int numMaterials, unsigned char* materialBase, int materialStride, + int* triangleMaterialsBase, int materialIndexStride); + + virtual ~btTriangleIndexVertexMaterialArray() {} + + void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) + { + m_materials.push_back(mat); + m_materials[m_materials.size()-1].m_triangleType = triangleType; + } + + virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType ,int subpart = 0); + + virtual void getLockedReadOnlyMaterialBase(const unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, + const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0); + +} +; + +#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index cf64071c1..ada20d3ef 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -1,943 +1,943 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson, 2008 -*/ -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "btGjkEpa2.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa2_impl -{ - - // Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) - - - // Shorthands - typedef unsigned int U; - typedef unsigned char U1; - - // MinkowskiDiff - struct MinkowskiDiff - { - const btConvexShape* m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - btVector3 (btConvexShape::*Ls)(const btVector3&) const; - void EnableMargin(bool enable) - { - if(enable) - Ls=&btConvexShape::localGetSupportVertexNonVirtual; - else - Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; - } - inline btVector3 Support0(const btVector3& d) const - { - return(((m_shapes[0])->*(Ls))(d)); - } - inline btVector3 Support1(const btVector3& d) const - { - return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); - } - inline btVector3 Support(const btVector3& d) const - { - return(Support0(d)-Support1(-d)); - } - btVector3 Support(const btVector3& d,U index) const - { - if(index) - return(Support1(d)); - else - return(Support0(d)); - } - }; - - typedef MinkowskiDiff tShape; - - - // GJK - struct GJK - { - /* Types */ - struct sSV - { - btVector3 d,w; - }; - struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; - struct eStatus { enum _ { - Valid, - Inside, - Failed };}; - /* Fields */ - tShape m_shape; - btVector3 m_ray; - btScalar m_distance; - sSimplex m_simplices[2]; - sSV m_store[4]; - sSV* m_free[4]; - U m_nfree; - U m_current; - sSimplex* m_simplex; - eStatus::_ m_status; - /* Methods */ - GJK() - { - Initialize(); - } - void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } - eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length2()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;iw*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - const btVector3 p=cross(d,axis); - if(p.length2()>0) - { - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - if(n.length2()>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; - } - return(false); - } - /* Internals */ - void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } - void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } - void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } - static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } - static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=cross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2]; - U subm; - for(U i=0;i<3;++i) - { - if(dot(*vt[i],cross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]; - U subm; - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd((subm&1?1<e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } - static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } - static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } - - - void Initialize() - { - m_status = eStatus::Failed; - m_normal = btVector3(0,0,0); - m_depth = 0; - m_nextsv = 0; - for(U i=0;i1)&&gjk.EncloseOrigin()) - { - - /* Clean up */ - while(m_hull.root) - { - sFace* f = m_hull.root; - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } - sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - dot(a->w,cross(face->n,a->w-b->w)), - dot(b->w,cross(face->n,b->w-c->w))), - dot(c->w,cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = dot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); - } - sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqdp; - } - } - return(minf); - } - bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } - - }; - - // - static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - btGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) - { - /* Results */ - results.witnesses[0] = - results.witnesses[1] = btVector3(0,0,0); - results.status = btGjkEpaSolver2::sResults::Separated; - /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); - shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); - shape.EnableMargin(withmargins); - } - -} - -// -// Api -// - -using namespace gjkepa2_impl; - -// -int btGjkEpaSolver2::StackSizeRequirement() -{ - return(sizeof(GJK)+sizeof(EPA)); -} - -// -bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); - if(gjk_status==GJK::eStatus::Valid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) - { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - results.normal = w0-w1; - results.distance = results.normal.length(); - results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; - return(true); - } - else - { - results.status = gjk_status==GJK::eStatus::Inside? - sResults::Penetrating : - sResults::GJK_Failed ; - return(false); - } -} - -// -bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins) -{ - tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); - switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - results.normal = -epa.m_normal; - results.distance = -epa.m_depth; - return(true); - } else results.status=sResults::EPA_Failed; - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - } - return(false); -} - -// -btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, - btScalar margin, - const btConvexShape* shape0, - const btTransform& wtrs0, - sResults& results) -{ - tShape shape; - btSphereShape shape1(margin); - btTransform wtrs1(btQuaternion(0,0,0,1),position); - Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); - GJK gjk; - GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); - if(gjk_status==GJK::eStatus::Valid) - { - btVector3 w0=btVector3(0,0,0); - btVector3 w1=btVector3(0,0,0); - for(U i=0;irank;++i) - { - const btScalar p=gjk.m_simplex->p[i]; - w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; - w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; - } - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*w1; - const btVector3 delta= results.witnesses[1]- - results.witnesses[0]; - const btScalar margin= shape0->getMarginNonVirtual()+ - shape1.getMarginNonVirtual(); - const btScalar length= delta.length(); - results.normal = delta/length; - results.witnesses[0] += results.normal*margin; - return(length-margin); - } - else - { - if(gjk_status==GJK::eStatus::Inside) - { - if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) - { - const btVector3 delta= results.witnesses[0]- - results.witnesses[1]; - const btScalar length= delta.length(); - if (length >= SIMD_EPSILON) - results.normal = delta/length; - return(-length); - } - } - } - return(SIMD_INFINITY); -} - -// -bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, - const btTransform& wtrs0, - const btConvexShape* shape1, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ - if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) - return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); - else - return(true); -} - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "btGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_impl +{ + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + + // Shorthands + typedef unsigned int U; + typedef unsigned char U1; + + // MinkowskiDiff + struct MinkowskiDiff + { + const btConvexShape* m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) + { + if(enable) + Ls=&btConvexShape::localGetSupportVertexNonVirtual; + else + Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual; + } + inline btVector3 Support0(const btVector3& d) const + { + return(((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + } + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + if(index) + return(Support1(d)); + else + return(Support0(d)); + } + }; + + typedef MinkowskiDiff tShape; + + + // GJK + struct GJK + { + /* Types */ + struct sSV + { + btVector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + btVector3 m_ray; + btScalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() + { + Initialize(); + } + void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + const btVector3 p=cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } + static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd((subm&1?1<e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = btVector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } + sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqdp; + } + } + return(minf); + } + bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + // + static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + btGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = btVector3(0,0,0); + results.status = btGjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); + shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); + shape.EnableMargin(withmargins); + } + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int btGjkEpaSolver2::StackSizeRequirement() +{ + return(sizeof(GJK)+sizeof(EPA)); +} + +// +bool btGjkEpaSolver2::Distance( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins) +{ + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;id,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + } + return(false); +} + +// +btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position, + btScalar margin, + const btConvexShape* shape0, + const btTransform& wtrs0, + sResults& results) +{ + tShape shape; + btSphereShape shape1(margin); + btTransform wtrs1(btQuaternion(0,0,0,1),position); + Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1)); + if(gjk_status==GJK::eStatus::Valid) + { + btVector3 w0=btVector3(0,0,0); + btVector3 w1=btVector3(0,0,0); + for(U i=0;irank;++i) + { + const btScalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const btVector3 delta= results.witnesses[1]- + results.witnesses[0]; + const btScalar margin= shape0->getMarginNonVirtual()+ + shape1.getMarginNonVirtual(); + const btScalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const btVector3 delta= results.witnesses[0]- + results.witnesses[1]; + const btScalar length= delta.length(); + if (length >= SIMD_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } + return(SIMD_INFINITY); +} + +// +bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const btTransform& wtrs0, + const btConvexShape* shape1, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ + if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index ff083ba41..a55214203 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -1,71 +1,71 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the -use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it -freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not -claim that you wrote the original software. If you use this software in a -product, an acknowledgment in the product documentation would be appreciated -but is not required. -2. Altered source versions must be plainly marked as such, and must not be -misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -GJK-EPA collision solver by Nathanael Presson, 2008 -*/ -#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ -#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ -#include "BulletCollision/CollisionShapes/btConvexShape.h" - -///btGjkEpaSolver contributed under zlib by Nathanael Presson -struct btGjkEpaSolver2 -{ -struct sResults - { - enum eStatus - { - Separated, /* Shapes doesnt penetrate */ - Penetrating, /* Shapes are penetrating */ - GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ - EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ - } status; - btVector3 witnesses[2]; - btVector3 normal; - btScalar distance; - }; - -static int StackSizeRequirement(); - -static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); - -static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results, - bool usemargins=true); - -static btScalar SignedDistance( const btVector3& position, - btScalar margin, - const btConvexShape* shape, - const btTransform& wtrs, - sResults& results); - -static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, - const btConvexShape* shape1,const btTransform& wtrs1, - const btVector3& guess, - sResults& results); -}; - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#include "BulletCollision/CollisionShapes/btConvexShape.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct btGjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + btVector3 witnesses[2]; + btVector3 normal; + btScalar distance; + }; + +static int StackSizeRequirement(); + +static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); + +static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results, + bool usemargins=true); + +static btScalar SignedDistance( const btVector3& position, + btScalar margin, + const btConvexShape* shape, + const btTransform& wtrs, + sResults& results); + +static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, + const btConvexShape* shape1,const btTransform& wtrs1, + const btVector3& guess, + sResults& results); +}; + +#endif diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 6d882cb3c..05573c7cf 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -1,54 +1,54 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -EPA Copyright (c) Ricardo Padrela 2006 - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "btGjkEpaPenetrationDepthSolver.h" - - -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) -{ - - (void)debugDraw; - (void)v; - (void)simplexSolver; - - const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); - btGjkEpaSolver2::sResults results; - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - return true; - } - - return false; -} - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "btGjkEpaPenetrationDepthSolver.h" + + +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" + +bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) +{ + + (void)debugDraw; + (void)v; + (void)simplexSolver; + + const btScalar radialmargin(btScalar(0.)); + + btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); + btGjkEpaSolver2::sResults results; + if(btGjkEpaSolver2::Penetration(pConvexA,transformA, + pConvexB,transformB, + guessVector,results)) + + { + // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + return true; + } + + return false; +} + + diff --git a/src/BulletDynamics/Character/btCharacterControllerInterface.h b/src/BulletDynamics/Character/btCharacterControllerInterface.h index ba4995d5f..a5d5cc069 100644 --- a/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -1,43 +1,43 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef CHARACTER_CONTROLLER_INTERFACE_H -#define CHARACTER_CONTROLLER_INTERFACE_H - -#include "LinearMath/btVector3.h" - -class btCollisionShape; -class btRigidBody; -class btCollisionWorld; - -class btCharacterControllerInterface -{ -public: - btCharacterControllerInterface () {}; - virtual ~btCharacterControllerInterface () {}; - - virtual void setWalkDirection(const btVector3& walkDirection) = 0; - virtual void reset () = 0; - virtual void warp (const btVector3& origin) = 0; - - virtual void preStep ( btCollisionWorld* collisionWorld) = 0; - virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; - virtual bool canJump () const = 0; - virtual void jump () = 0; - - virtual bool onGround () const = 0; -}; - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CHARACTER_CONTROLLER_INTERFACE_H +#define CHARACTER_CONTROLLER_INTERFACE_H + +#include "LinearMath/btVector3.h" + +class btCollisionShape; +class btRigidBody; +class btCollisionWorld; + +class btCharacterControllerInterface +{ +public: + btCharacterControllerInterface () {}; + virtual ~btCharacterControllerInterface () {}; + + virtual void setWalkDirection(const btVector3& walkDirection) = 0; + virtual void reset () = 0; + virtual void warp (const btVector3& origin) = 0; + + virtual void preStep ( btCollisionWorld* collisionWorld) = 0; + virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; + virtual bool canJump () const = 0; + virtual void jump () = 0; + + virtual bool onGround () const = 0; +}; + +#endif diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.cpp b/src/BulletDynamics/Character/btKinematicCharacterController.cpp index b8ef59b7c..18164303d 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.cpp +++ b/src/BulletDynamics/Character/btKinematicCharacterController.cpp @@ -1,471 +1,471 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/CollisionDispatch/btGhostObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "LinearMath/btDefaultMotionState.h" -#include "btKinematicCharacterController.h" - -static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; - -///@todo Interact with dynamic objects, -///Ride kinematicly animated platforms properly -///More realistic (or maybe just a config option) falling -/// -> Should integrate falling velocity manually and use that in stepDown() -///Support jumping -///Support ducking -class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback -{ -public: - btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - { - m_me = me; - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) - { - if (rayResult.m_collisionObject == m_me) - return 1.0; - - return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; -}; - -class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback -{ -public: - btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) - { - m_me = me; - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) - { - if (convexResult.m_hitCollisionObject == m_me) - return 1.0; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } -protected: - btCollisionObject* m_me; -}; - -/* - * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' - * - * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html - */ -btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) -{ - return direction - (btScalar(2.0) * direction.dot(normal)) * normal; -} - -/* - * Returns the portion of 'direction' that is parallel to 'normal' - */ -btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) -{ - btScalar magnitude = direction.dot(normal); - return normal * magnitude; -} - -/* - * Returns the portion of 'direction' that is perpindicular to 'normal' - */ -btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) -{ - return direction - parallelComponent(direction, normal); -} - -btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) -{ - m_upAxis = upAxis; - m_addedMargin = 0.02f; - m_walkDirection.setValue(0,0,0); - m_useGhostObjectSweepTest = true; - m_ghostObject = ghostObject; - m_stepHeight = stepHeight; - m_turnAngle = btScalar(0.0); - m_convexShape=convexShape; -} - -btKinematicCharacterController::~btKinematicCharacterController () -{ -} - -btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() -{ - return m_ghostObject; -} - -bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld) -{ - - bool penetration = false; - - collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - - btScalar maxPen = btScalar(0.0); - for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) - { - m_manifoldArray.resize(0); - - btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; - - if (collisionPair->m_algorithm) - collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); - - - for (int j=0;jgetBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); - for (int p=0;pgetNumContacts();p++) - { - const btManifoldPoint&pt = manifold->getContactPoint(p); - - if (pt.getDistance() < 0.0) - { - if (pt.getDistance() < maxPen) - { - maxPen = pt.getDistance(); - m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? - - } - m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); - penetration = true; - } else { - //printf("touching %f\n", pt.getDistance()); - } - } - - //manifold->clearManifold(); - } - } - btTransform newTrans = m_ghostObject->getWorldTransform(); - newTrans.setOrigin(m_currentPosition); - m_ghostObject->setWorldTransform(newTrans); -// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); - return penetration; -} - -void btKinematicCharacterController::stepUp ( btCollisionWorld* world) -{ - // phase 1: up - btTransform start, end; - m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight; - - start.setIdentity (); - end.setIdentity (); - - /* FIXME: Handle penetration properly */ - start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f)); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); - } - else - { - world->convexSweepTest (m_convexShape, start, end, callback); - } - - if (callback.hasHit()) - { - // we moved up only a fraction of the step height - m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - } else { - m_currentStepOffset = m_stepHeight; - m_currentPosition = m_targetPosition; - } -} - -void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) -{ - btVector3 movementDirection = m_targetPosition - m_currentPosition; - btScalar movementLength = movementDirection.length(); - if (movementLength>SIMD_EPSILON) - { - movementDirection.normalize(); - - btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); - reflectDir.normalize(); - - btVector3 parallelDir, perpindicularDir; - - parallelDir = parallelComponent (reflectDir, hitNormal); - perpindicularDir = perpindicularComponent (reflectDir, hitNormal); - - m_targetPosition = m_currentPosition; - if (0)//tangentMag != 0.0) - { - btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); -// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); - m_targetPosition += parComponent; - } - - if (normalMag != 0.0) - { - btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); -// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); - m_targetPosition += perpComponent; - } - } else - { -// printf("movementLength don't normalize a zero vector\n"); - } -} - -void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) -{ - - btVector3 originalDir = walkMove.normalized(); - if (walkMove.length() < SIMD_EPSILON) - { - originalDir.setValue(0.f,0.f,0.f); - } -// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); - // phase 2: forward and strafe - btTransform start, end; - m_targetPosition = m_currentPosition + walkMove; - start.setIdentity (); - end.setIdentity (); - - btScalar fraction = 1.0; - btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); -// printf("distance2=%f\n",distance2); - - if (m_touchingContact) - { - if (originalDir.dot(m_touchingNormal) > btScalar(0.0)) - updateTargetPositionBasedOnCollision (m_touchingNormal); - } - - int maxIter = 10; - - while (fraction > btScalar(0.01) && maxIter-- > 0) - { - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - - btScalar margin = m_convexShape->getMargin(); - m_convexShape->setMargin(margin + m_addedMargin); - - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - - m_convexShape->setMargin(margin); - - - fraction -= callback.m_closestHitFraction; - - if (callback.hasHit()) - { - // we moved only a fraction - btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); - if (hitDistance<0.f) - { -// printf("neg dist?\n"); - } - - /* If the distance is farther than the collision margin, move */ - if (hitDistance > m_addedMargin) - { -// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - } - - updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); - btVector3 currentDir = m_targetPosition - m_currentPosition; - distance2 = currentDir.length2(); - if (distance2 > SIMD_EPSILON) - { - currentDir.normalize(); - /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ - if (currentDir.dot(originalDir) <= btScalar(0.0)) - { - break; - } - } else - { -// printf("currentDir: don't normalize a zero vector\n"); - break; - } - } else { - // we moved whole way - m_currentPosition = m_targetPosition; - } - - // if (callback.m_closestHitFraction == 0.f) - // break; - - } -} - -void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) -{ - btTransform start, end; - - // phase 3: down - btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset; - btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; - m_targetPosition -= (step_drop + gravity_drop); - - start.setIdentity (); - end.setIdentity (); - - start.setOrigin (m_currentPosition); - end.setOrigin (m_targetPosition); - - btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); - callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; - callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; - - if (m_useGhostObjectSweepTest) - { - m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } else - { - collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); - } - - if (callback.hasHit()) - { - // we dropped a fraction of the height -> hit floor - m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); - } else { - // we dropped the full height - - m_currentPosition = m_targetPosition; - } -} - -void btKinematicCharacterController::reset () -{ -} - -void btKinematicCharacterController::warp (const btVector3& origin) -{ - btTransform xform; - xform.setIdentity(); - xform.setOrigin (origin); - m_ghostObject->setWorldTransform (xform); -} - - -void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) -{ - - int numPenetrationLoops = 0; - m_touchingContact = false; - while (recoverFromPenetration (collisionWorld)) - { - numPenetrationLoops++; - m_touchingContact = true; - if (numPenetrationLoops > 4) - { -// printf("character could not recover from penetration = %d\n", numPenetrationLoops); - break; - } - } - - m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); - m_targetPosition = m_currentPosition; -// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); - - -} - -void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) -{ - btTransform xform; - xform = m_ghostObject->getWorldTransform (); - -// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); -// printf("walkSpeed=%f\n",walkSpeed); - - stepUp (collisionWorld); - stepForwardAndStrafe (collisionWorld, m_walkDirection); - stepDown (collisionWorld, dt); - - xform.setOrigin (m_currentPosition); - m_ghostObject->setWorldTransform (xform); -} - -void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) -{ - m_fallSpeed = fallSpeed; -} - -void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) -{ - m_jumpSpeed = jumpSpeed; -} - -void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) -{ - m_maxJumpHeight = maxJumpHeight; -} - -bool btKinematicCharacterController::canJump () const -{ - return onGround(); -} - -void btKinematicCharacterController::jump () -{ - if (!canJump()) - return; - -#if 0 - currently no jumping. - btTransform xform; - m_rigidBody->getMotionState()->getWorldTransform (xform); - btVector3 up = xform.getBasis()[1]; - up.normalize (); - btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); - m_rigidBody->applyCentralImpulse (up * magnitude); -#endif -} - -bool btKinematicCharacterController::onGround () const -{ - return true; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionDispatch/btGhostObject.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "LinearMath/btDefaultMotionState.h" +#include "btKinematicCharacterController.h" + +static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) }; + +///@todo Interact with dynamic objects, +///Ride kinematicly animated platforms properly +///More realistic (or maybe just a config option) falling +/// -> Should integrate falling velocity manually and use that in stepDown() +///Support jumping +///Support ducking +class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback +{ +public: + btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + { + m_me = me; + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) + { + if (rayResult.m_collisionObject == m_me) + return 1.0; + + return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); + } +protected: + btCollisionObject* m_me; +}; + +class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback +{ +public: + btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) + { + m_me = me; + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return 1.0; + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } +protected: + btCollisionObject* m_me; +}; + +/* + * Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' + * + * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html + */ +btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) +{ + return direction - (btScalar(2.0) * direction.dot(normal)) * normal; +} + +/* + * Returns the portion of 'direction' that is parallel to 'normal' + */ +btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) +{ + btScalar magnitude = direction.dot(normal); + return normal * magnitude; +} + +/* + * Returns the portion of 'direction' that is perpindicular to 'normal' + */ +btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) +{ + return direction - parallelComponent(direction, normal); +} + +btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) +{ + m_upAxis = upAxis; + m_addedMargin = 0.02f; + m_walkDirection.setValue(0,0,0); + m_useGhostObjectSweepTest = true; + m_ghostObject = ghostObject; + m_stepHeight = stepHeight; + m_turnAngle = btScalar(0.0); + m_convexShape=convexShape; +} + +btKinematicCharacterController::~btKinematicCharacterController () +{ +} + +btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() +{ + return m_ghostObject; +} + +bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld) +{ + + bool penetration = false; + + collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + + btScalar maxPen = btScalar(0.0); + for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) + { + m_manifoldArray.resize(0); + + btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; + + if (collisionPair->m_algorithm) + collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); + + + for (int j=0;jgetBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); + for (int p=0;pgetNumContacts();p++) + { + const btManifoldPoint&pt = manifold->getContactPoint(p); + + if (pt.getDistance() < 0.0) + { + if (pt.getDistance() < maxPen) + { + maxPen = pt.getDistance(); + m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? + + } + m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); + penetration = true; + } else { + //printf("touching %f\n", pt.getDistance()); + } + } + + //manifold->clearManifold(); + } + } + btTransform newTrans = m_ghostObject->getWorldTransform(); + newTrans.setOrigin(m_currentPosition); + m_ghostObject->setWorldTransform(newTrans); +// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); + return penetration; +} + +void btKinematicCharacterController::stepUp ( btCollisionWorld* world) +{ + // phase 1: up + btTransform start, end; + m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight; + + start.setIdentity (); + end.setIdentity (); + + /* FIXME: Handle penetration properly */ + start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f)); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); + } + else + { + world->convexSweepTest (m_convexShape, start, end, callback); + } + + if (callback.hasHit()) + { + // we moved up only a fraction of the step height + m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } else { + m_currentStepOffset = m_stepHeight; + m_currentPosition = m_targetPosition; + } +} + +void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) +{ + btVector3 movementDirection = m_targetPosition - m_currentPosition; + btScalar movementLength = movementDirection.length(); + if (movementLength>SIMD_EPSILON) + { + movementDirection.normalize(); + + btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); + reflectDir.normalize(); + + btVector3 parallelDir, perpindicularDir; + + parallelDir = parallelComponent (reflectDir, hitNormal); + perpindicularDir = perpindicularComponent (reflectDir, hitNormal); + + m_targetPosition = m_currentPosition; + if (0)//tangentMag != 0.0) + { + btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); +// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); + m_targetPosition += parComponent; + } + + if (normalMag != 0.0) + { + btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); +// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); + m_targetPosition += perpComponent; + } + } else + { +// printf("movementLength don't normalize a zero vector\n"); + } +} + +void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) +{ + + btVector3 originalDir = walkMove.normalized(); + if (walkMove.length() < SIMD_EPSILON) + { + originalDir.setValue(0.f,0.f,0.f); + } +// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); + // phase 2: forward and strafe + btTransform start, end; + m_targetPosition = m_currentPosition + walkMove; + start.setIdentity (); + end.setIdentity (); + + btScalar fraction = 1.0; + btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); +// printf("distance2=%f\n",distance2); + + if (m_touchingContact) + { + if (originalDir.dot(m_touchingNormal) > btScalar(0.0)) + updateTargetPositionBasedOnCollision (m_touchingNormal); + } + + int maxIter = 10; + + while (fraction > btScalar(0.01) && maxIter-- > 0) + { + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + + btScalar margin = m_convexShape->getMargin(); + m_convexShape->setMargin(margin + m_addedMargin); + + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } else + { + collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + + m_convexShape->setMargin(margin); + + + fraction -= callback.m_closestHitFraction; + + if (callback.hasHit()) + { + // we moved only a fraction + btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); + if (hitDistance<0.f) + { +// printf("neg dist?\n"); + } + + /* If the distance is farther than the collision margin, move */ + if (hitDistance > m_addedMargin) + { +// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } + + updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); + btVector3 currentDir = m_targetPosition - m_currentPosition; + distance2 = currentDir.length2(); + if (distance2 > SIMD_EPSILON) + { + currentDir.normalize(); + /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ + if (currentDir.dot(originalDir) <= btScalar(0.0)) + { + break; + } + } else + { +// printf("currentDir: don't normalize a zero vector\n"); + break; + } + } else { + // we moved whole way + m_currentPosition = m_targetPosition; + } + + // if (callback.m_closestHitFraction == 0.f) + // break; + + } +} + +void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) +{ + btTransform start, end; + + // phase 3: down + btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset; + btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; + m_targetPosition -= (step_drop + gravity_drop); + + start.setIdentity (); + end.setIdentity (); + + start.setOrigin (m_currentPosition); + end.setOrigin (m_targetPosition); + + btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); + callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; + callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; + + if (m_useGhostObjectSweepTest) + { + m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } else + { + collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); + } + + if (callback.hasHit()) + { + // we dropped a fraction of the height -> hit floor + m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); + } else { + // we dropped the full height + + m_currentPosition = m_targetPosition; + } +} + +void btKinematicCharacterController::reset () +{ +} + +void btKinematicCharacterController::warp (const btVector3& origin) +{ + btTransform xform; + xform.setIdentity(); + xform.setOrigin (origin); + m_ghostObject->setWorldTransform (xform); +} + + +void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) +{ + + int numPenetrationLoops = 0; + m_touchingContact = false; + while (recoverFromPenetration (collisionWorld)) + { + numPenetrationLoops++; + m_touchingContact = true; + if (numPenetrationLoops > 4) + { +// printf("character could not recover from penetration = %d\n", numPenetrationLoops); + break; + } + } + + m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); + m_targetPosition = m_currentPosition; +// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); + + +} + +void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) +{ + btTransform xform; + xform = m_ghostObject->getWorldTransform (); + +// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); +// printf("walkSpeed=%f\n",walkSpeed); + + stepUp (collisionWorld); + stepForwardAndStrafe (collisionWorld, m_walkDirection); + stepDown (collisionWorld, dt); + + xform.setOrigin (m_currentPosition); + m_ghostObject->setWorldTransform (xform); +} + +void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) +{ + m_fallSpeed = fallSpeed; +} + +void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) +{ + m_jumpSpeed = jumpSpeed; +} + +void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) +{ + m_maxJumpHeight = maxJumpHeight; +} + +bool btKinematicCharacterController::canJump () const +{ + return onGround(); +} + +void btKinematicCharacterController::jump () +{ + if (!canJump()) + return; + +#if 0 + currently no jumping. + btTransform xform; + m_rigidBody->getMotionState()->getWorldTransform (xform); + btVector3 up = xform.getBasis()[1]; + up.normalize (); + btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); + m_rigidBody->applyCentralImpulse (up * magnitude); +#endif +} + +bool btKinematicCharacterController::onGround () const +{ + return true; +} diff --git a/src/BulletDynamics/Character/btKinematicCharacterController.h b/src/BulletDynamics/Character/btKinematicCharacterController.h index 8489e4f7c..932e428f5 100644 --- a/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -1,116 +1,116 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef KINEMATIC_CHARACTER_CONTROLLER_H -#define KINEMATIC_CHARACTER_CONTROLLER_H - -#include "LinearMath/btVector3.h" - -#include "btCharacterControllerInterface.h" - -class btCollisionShape; -class btRigidBody; -class btCollisionWorld; -class btCollisionDispatcher; -class btPairCachingGhostObject; - -///btKinematicCharacterController is an object that supports a sliding motion in a world. -///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. -///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. -class btKinematicCharacterController : public btCharacterControllerInterface -{ -protected: - btScalar m_halfHeight; - - btPairCachingGhostObject* m_ghostObject; - btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast - - btScalar m_fallSpeed; - btScalar m_jumpSpeed; - btScalar m_maxJumpHeight; - - btScalar m_turnAngle; - - btScalar m_stepHeight; - - btScalar m_addedMargin;//@todo: remove this and fix the code - - ///this is the desired walk direction, set by the user - btVector3 m_walkDirection; - - //some internal variables - btVector3 m_currentPosition; - btScalar m_currentStepOffset; - btVector3 m_targetPosition; - - ///keep track of the contact manifolds - btManifoldArray m_manifoldArray; - - bool m_touchingContact; - btVector3 m_touchingNormal; - - bool m_useGhostObjectSweepTest; - - int m_upAxis; - - btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); - btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); - btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal); - - bool recoverFromPenetration (btCollisionWorld* collisionWorld); - void stepUp (btCollisionWorld* collisionWorld); - void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); - void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); - void stepDown (btCollisionWorld* collisionWorld, btScalar dt); -public: - btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); - ~btKinematicCharacterController (); - - void setUpAxis (int axis) - { - if (axis < 0) - axis = 0; - if (axis > 2) - axis = 2; - m_upAxis = axis; - } - - virtual void setWalkDirection(const btVector3& walkDirection) - { - m_walkDirection = walkDirection; - } - - void reset (); - void warp (const btVector3& origin); - - void preStep ( btCollisionWorld* collisionWorld); - void playerStep (btCollisionWorld* collisionWorld, btScalar dt); - - void setFallSpeed (btScalar fallSpeed); - void setJumpSpeed (btScalar jumpSpeed); - void setMaxJumpHeight (btScalar maxJumpHeight); - bool canJump () const; - void jump (); - - btPairCachingGhostObject* getGhostObject(); - void setUseGhostSweepTest(bool useGhostObjectSweepTest) - { - m_useGhostObjectSweepTest = useGhostObjectSweepTest; - } - - bool onGround () const; -}; - -#endif // KINEMATIC_CHARACTER_CONTROLLER_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef KINEMATIC_CHARACTER_CONTROLLER_H +#define KINEMATIC_CHARACTER_CONTROLLER_H + +#include "LinearMath/btVector3.h" + +#include "btCharacterControllerInterface.h" + +class btCollisionShape; +class btRigidBody; +class btCollisionWorld; +class btCollisionDispatcher; +class btPairCachingGhostObject; + +///btKinematicCharacterController is an object that supports a sliding motion in a world. +///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations. +///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user. +class btKinematicCharacterController : public btCharacterControllerInterface +{ +protected: + btScalar m_halfHeight; + + btPairCachingGhostObject* m_ghostObject; + btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast + + btScalar m_fallSpeed; + btScalar m_jumpSpeed; + btScalar m_maxJumpHeight; + + btScalar m_turnAngle; + + btScalar m_stepHeight; + + btScalar m_addedMargin;//@todo: remove this and fix the code + + ///this is the desired walk direction, set by the user + btVector3 m_walkDirection; + + //some internal variables + btVector3 m_currentPosition; + btScalar m_currentStepOffset; + btVector3 m_targetPosition; + + ///keep track of the contact manifolds + btManifoldArray m_manifoldArray; + + bool m_touchingContact; + btVector3 m_touchingNormal; + + bool m_useGhostObjectSweepTest; + + int m_upAxis; + + btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); + btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); + btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal); + + bool recoverFromPenetration (btCollisionWorld* collisionWorld); + void stepUp (btCollisionWorld* collisionWorld); + void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); + void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); + void stepDown (btCollisionWorld* collisionWorld, btScalar dt); +public: + btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); + ~btKinematicCharacterController (); + + void setUpAxis (int axis) + { + if (axis < 0) + axis = 0; + if (axis > 2) + axis = 2; + m_upAxis = axis; + } + + virtual void setWalkDirection(const btVector3& walkDirection) + { + m_walkDirection = walkDirection; + } + + void reset (); + void warp (const btVector3& origin); + + void preStep ( btCollisionWorld* collisionWorld); + void playerStep (btCollisionWorld* collisionWorld, btScalar dt); + + void setFallSpeed (btScalar fallSpeed); + void setJumpSpeed (btScalar jumpSpeed); + void setMaxJumpHeight (btScalar maxJumpHeight); + bool canJump () const; + void jump (); + + btPairCachingGhostObject* getGhostObject(); + void setUseGhostSweepTest(bool useGhostObjectSweepTest) + { + m_useGhostObjectSweepTest = useGhostObjectSweepTest; + } + + bool onGround () const; +}; + +#endif // KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index 1daea39f4..50d069603 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -1,817 +1,817 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 -*/ - -//----------------------------------------------------------------------------- - -#include "btSliderConstraint.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btTransformUtil.h" -#include - -//----------------------------------------------------------------------------- - -void btSliderConstraint::initParams() -{ - m_lowerLinLimit = btScalar(1.0); - m_upperLinLimit = btScalar(-1.0); - m_lowerAngLimit = btScalar(0.); - m_upperAngLimit = btScalar(0.); - m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirLin = btScalar(0.); - m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingDirAng = btScalar(0.); - m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; - m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; - m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; - m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; - - m_poweredLinMotor = false; - m_targetLinMotorVelocity = btScalar(0.); - m_maxLinMotorForce = btScalar(0.); - m_accumulatedLinMotorImpulse = btScalar(0.0); - - m_poweredAngMotor = false; - m_targetAngMotorVelocity = btScalar(0.); - m_maxAngMotorForce = btScalar(0.); - m_accumulatedAngMotorImpulse = btScalar(0.0); - -} // btSliderConstraint::initParams() - -//----------------------------------------------------------------------------- - -btSliderConstraint::btSliderConstraint() - :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), - m_useLinearReferenceFrameA(true), - m_useSolveConstraintObsolete(false) -// m_useSolveConstraintObsolete(true) -{ - initParams(); -} // btSliderConstraint::btSliderConstraint() - -//----------------------------------------------------------------------------- - -btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) - : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) - , m_frameInA(frameInA) - , m_frameInB(frameInB), - m_useLinearReferenceFrameA(useLinearReferenceFrameA), - m_useSolveConstraintObsolete(false) -// m_useSolveConstraintObsolete(true) -{ - initParams(); -} // btSliderConstraint::btSliderConstraint() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::buildJacobian() -{ - if (!m_useSolveConstraintObsolete) - { - return; - } - if(m_useLinearReferenceFrameA) - { - buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); - } - else - { - buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); - } -} // btSliderConstraint::buildJacobian() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) -{ - //calculate transforms - m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; - m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); - m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacLin[i]) btJacobianEntry( - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - m_relPosA, - m_relPosB, - normalWorld, - rbA.getInvInertiaDiagLocal(), - rbA.getInvMass(), - rbB.getInvInertiaDiagLocal(), - rbB.getInvMass() - ); - m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); - m_depth[i] = m_delta.dot(normalWorld); - } - testLinLimits(); - // angular part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacAng[i]) btJacobianEntry( - normalWorld, - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - rbA.getInvInertiaDiagLocal(), - rbB.getInvInertiaDiagLocal() - ); - } - testAngLimits(); - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); - // clear accumulator for motors - m_accumulatedLinMotorImpulse = btScalar(0.0); - m_accumulatedAngMotorImpulse = btScalar(0.0); -} // btSliderConstraint::buildJacobianInt() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::getInfo1(btConstraintInfo1* info) -{ - if (m_useSolveConstraintObsolete) - { - info->m_numConstraintRows = 0; - info->nub = 0; - } - else - { - info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular - info->nub = 2; - //prepare constraint - calculateTransforms(); - testLinLimits(); - if(getSolveLinLimit() || getPoweredLinMotor()) - { - info->m_numConstraintRows++; // limit 3rd linear as well - info->nub--; - } - testAngLimits(); - if(getSolveAngLimit() || getPoweredAngMotor()) - { - info->m_numConstraintRows++; // limit 3rd angular as well - info->nub--; - } - } -} // btSliderConstraint::getInfo1() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::getInfo2(btConstraintInfo2* info) -{ - btAssert(!m_useSolveConstraintObsolete); - int i, s = info->rowskip; - const btTransform& trA = getCalculatedTransformA(); - const btTransform& trB = getCalculatedTransformB(); - btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); - // make rotations around Y and Z equal - // the slider axis should be the only unconstrained - // rotational axis, the angular velocity of the two bodies perpendicular to - // the slider axis should be equal. thus the constraint equations are - // p*w1 - p*w2 = 0 - // q*w1 - q*w2 = 0 - // where p and q are unit vectors normal to the slider axis, and w1 and w2 - // are the angular velocity vectors of the two bodies. - // get slider axis (X) - btVector3 ax1 = trA.getBasis().getColumn(0); - // get 2 orthos to slider axis (Y, Z) - btVector3 p = trA.getBasis().getColumn(1); - btVector3 q = trA.getBasis().getColumn(2); - // set the two slider rows - info->m_J1angularAxis[0] = p[0]; - info->m_J1angularAxis[1] = p[1]; - info->m_J1angularAxis[2] = p[2]; - info->m_J1angularAxis[s+0] = q[0]; - info->m_J1angularAxis[s+1] = q[1]; - info->m_J1angularAxis[s+2] = q[2]; - - info->m_J2angularAxis[0] = -p[0]; - info->m_J2angularAxis[1] = -p[1]; - info->m_J2angularAxis[2] = -p[2]; - info->m_J2angularAxis[s+0] = -q[0]; - info->m_J2angularAxis[s+1] = -q[1]; - info->m_J2angularAxis[s+2] = -q[2]; - // compute the right hand side of the constraint equation. set relative - // body velocities along p and q to bring the slider back into alignment. - // if ax1,ax2 are the unit length slider axes as computed from body1 and - // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). - // if "theta" is the angle between ax1 and ax2, we need an angular velocity - // along u to cover angle erp*theta in one step : - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (erp*fps) * (ax1 x ax2) - // ax1 x ax2 is in the plane space of ax1, so we project the angular - // velocity to p and q to find the right hand side. - btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); - btVector3 ax2 = trB.getBasis().getColumn(0); - btVector3 u = ax1.cross(ax2); - info->m_constraintError[0] = k * u.dot(p); - info->m_constraintError[s] = k * u.dot(q); - // pull out pos and R for both bodies. also get the connection - // vector c = pos2-pos1. - // next two rows. we want: vel2 = vel1 + w1 x c ... but this would - // result in three equations, so we project along the planespace vectors - // so that sliding along the slider axis is disregarded. for symmetry we - // also consider rotation around center of mass of two bodies (factA and factB). - btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); - btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); - int s2 = 2 * s, s3 = 3 * s; - btVector3 c; - btScalar miA = m_rbA.getInvMass(); - btScalar miB = m_rbB.getInvMass(); - btScalar miS = miA + miB; - btScalar factA, factB; - if(miS > btScalar(0.f)) - { - factA = miB / miS; - } - else - { - factA = btScalar(0.5f); - } - if(factA > 0.99f) factA = 0.99f; - if(factA < 0.01f) factA = 0.01f; - factB = btScalar(1.0f) - factA; - c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); - btVector3 tmp = c.cross(p); - for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; - tmp = c.cross(q); - for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; - for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; - - for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; - for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; - // compute two elements of right hand side. we want to align the offset - // point (in body 2's frame) with the center of body 1. - btVector3 ofs; // offset point in global coordinates - ofs = trB.getOrigin() - trA.getOrigin(); - k = info->fps * info->erp * getSoftnessOrthoLin(); - info->m_constraintError[s2] = k * p.dot(ofs); - info->m_constraintError[s3] = k * q.dot(ofs); - int nrow = 3; // last filled row - int srow; - // check linear limits linear - btScalar limit_err = btScalar(0.0); - int limit = 0; - if(getSolveLinLimit()) - { - limit_err = getLinDepth() * signFact; - limit = (limit_err > btScalar(0.0)) ? 2 : 1; - } - int powered = 0; - if(getPoweredLinMotor()) - { - powered = 1; - } - // if the slider has joint limits or motor, add in the extra row - if (limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1linearAxis[srow+0] = ax1[0]; - info->m_J1linearAxis[srow+1] = ax1[1]; - info->m_J1linearAxis[srow+2] = ax1[2]; - // linear torque decoupling step: - // - // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in limited slider-jointed free - // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the center of mass of the two bodies - btVector3 ltd; // Linear Torque Decoupling vector (a torque) -// c = btScalar(0.5) * c; - ltd = c.cross(ax1); - info->m_J1angularAxis[srow+0] = factA*ltd[0]; - info->m_J1angularAxis[srow+1] = factA*ltd[1]; - info->m_J1angularAxis[srow+2] = factA*ltd[2]; - info->m_J2angularAxis[srow+0] = factB*ltd[0]; - info->m_J2angularAxis[srow+1] = factB*ltd[1]; - info->m_J2angularAxis[srow+2] = factB*ltd[2]; - // right-hand part - btScalar lostop = getLowerLinLimit(); - btScalar histop = getUpperLinLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - info->m_constraintError[srow] = 0.; - info->m_lowerLimit[srow] = 0.; - info->m_upperLimit[srow] = 0.; - if(powered) - { - info->cfm[nrow] = btScalar(0.0); - btScalar tag_vel = getTargetLinMotorVelocity(); - btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); -// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); - info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); - info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; - info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * info->erp; - info->m_constraintError[srow] += k * limit_err; - info->cfm[srow] = btScalar(0.0); // stop_cfm; - if(lostop == histop) - { // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - else - { // high limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_rbA.getLinearVelocity().dot(ax1); - vel -= m_rbB.getLinearVelocity().dot(ax1); - vel *= signFact; - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if (newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimLin(); - } // if(limit) - } // if linear limit - // check angular limits - limit_err = btScalar(0.0); - limit = 0; - if(getSolveAngLimit()) - { - limit_err = getAngDepth(); - limit = (limit_err > btScalar(0.0)) ? 1 : 2; - } - // if the slider has joint limits, add in the extra row - powered = 0; - if(getPoweredAngMotor()) - { - powered = 1; - } - if(limit || powered) - { - nrow++; - srow = nrow * info->rowskip; - info->m_J1angularAxis[srow+0] = ax1[0]; - info->m_J1angularAxis[srow+1] = ax1[1]; - info->m_J1angularAxis[srow+2] = ax1[2]; - - info->m_J2angularAxis[srow+0] = -ax1[0]; - info->m_J2angularAxis[srow+1] = -ax1[1]; - info->m_J2angularAxis[srow+2] = -ax1[2]; - - btScalar lostop = getLowerAngLimit(); - btScalar histop = getUpperAngLimit(); - if(limit && (lostop == histop)) - { // the joint motor is ineffective - powered = 0; - } - if(powered) - { - info->cfm[srow] = btScalar(0.0); - btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); - info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); - info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; - info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; - } - if(limit) - { - k = info->fps * info->erp; - info->m_constraintError[srow] += k * limit_err; - info->cfm[srow] = btScalar(0.0); // stop_cfm; - if(lostop == histop) - { - // limited low and high simultaneously - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else if(limit == 1) - { // low limit - info->m_lowerLimit[srow] = 0; - info->m_upperLimit[srow] = SIMD_INFINITY; - } - else - { // high limit - info->m_lowerLimit[srow] = -SIMD_INFINITY; - info->m_upperLimit[srow] = 0; - } - // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) - btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); - if(bounce > btScalar(0.0)) - { - btScalar vel = m_rbA.getAngularVelocity().dot(ax1); - vel -= m_rbB.getAngularVelocity().dot(ax1); - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if(limit == 1) - { // low limit - if(vel < 0) - { - btScalar newc = -bounce * vel; - if(newc > info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - else - { // high limit - all those computations are reversed - if(vel > 0) - { - btScalar newc = -bounce * vel; - if(newc < info->m_constraintError[srow]) - { - info->m_constraintError[srow] = newc; - } - } - } - } - info->m_constraintError[srow] *= getSoftnessLimAng(); - } // if(limit) - } // if angular limit or powered -} // btSliderConstraint::getInfo2() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - if (m_useSolveConstraintObsolete) - { - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) - { - solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); - } - else - { - solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); - } - } -} // btSliderConstraint::solveConstraint() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) -{ - int i; - // linear - btVector3 velA; - bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); - btVector3 velB; - bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); - btVector3 vel = velA - velB; - for(i = 0; i < 3; i++) - { - const btVector3& normal = m_jacLin[i].m_linearJointAxis; - btScalar rel_vel = normal.dot(vel); - // calculate positional error - btScalar depth = m_depth[i]; - // get parameters - btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // calcutate and apply impulse - btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; - btVector3 impulse_vector = normal * normalImpulse; - - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - if(m_poweredLinMotor && (!i)) - { // apply linear motor - if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) - { - btScalar desiredMotorVel = m_targetLinMotorVelocity; - btScalar motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); - if(new_acc > m_maxLinMotorForce) - { - new_acc = m_maxLinMotorForce; - } - btScalar del = new_acc - m_accumulatedLinMotorImpulse; - if(normalImpulse < btScalar(0.0)) - { - normalImpulse = -del; - } - else - { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - } - } - } - // angular - // get axes in world space - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - - btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); - - btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundAxisA; - btVector3 angBorthog = angVelB - angVelAroundAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - //solve orthogonal angular velocity correction - btScalar len = velrelOrthog.length(); - btScalar orthorImpulseMag = 0.f; - - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); - btVector3 angularAxis = angularError; - btScalar angularImpulseMag = 0; - - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - angularError *= angularImpulseMag; - } - // apply impulse - //rbA.applyTorqueImpulse(-velrelOrthog+angularError); - //rbB.applyTorqueImpulse(velrelOrthog-angularError); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); - - - btScalar impulseMag; - //solve angular limits - if(m_solveAngLim) - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessLimAng; - } - else - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessDirAng; - } - btVector3 impulse = axisA * impulseMag; - //rbA.applyTorqueImpulse(impulse); - //rbB.applyTorqueImpulse(-impulse); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); - - - - //apply angular motor - if(m_poweredAngMotor) - { - if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) - { - btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - btScalar projRelVel = velrel.dot(axisA); - - btScalar desiredMotorVel = m_targetAngMotorVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; - - btScalar angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); - if(new_acc > m_maxAngMotorForce) - { - new_acc = m_maxAngMotorForce; - } - btScalar del = new_acc - m_accumulatedAngMotorImpulse; - if(angImpulse < btScalar(0.0)) - { - angImpulse = -del; - } - else - { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - btVector3 motorImp = angImpulse * axisA; - //rbA.applyTorqueImpulse(motorImp); - //rbB.applyTorqueImpulse(-motorImp); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); - } - } -} // btSliderConstraint::solveConstraint() - -//----------------------------------------------------------------------------- - -//----------------------------------------------------------------------------- - -void btSliderConstraint::calculateTransforms(void){ - if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) - { - m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; - m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; - } - else - { - m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; - m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; - } - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) - { - m_delta = m_realPivotBInW - m_realPivotAInW; - } - else - { - m_delta = m_realPivotAInW - m_realPivotBInW; - } - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - m_depth[i] = m_delta.dot(normalWorld); - } -} // btSliderConstraint::calculateTransforms() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::testLinLimits(void) -{ - m_solveLinLim = false; - m_linPos = m_depth[0]; - if(m_lowerLinLimit <= m_upperLinLimit) - { - if(m_depth[0] > m_upperLinLimit) - { - m_depth[0] -= m_upperLinLimit; - m_solveLinLim = true; - } - else if(m_depth[0] < m_lowerLinLimit) - { - m_depth[0] -= m_lowerLinLimit; - m_solveLinLim = true; - } - else - { - m_depth[0] = btScalar(0.); - } - } - else - { - m_depth[0] = btScalar(0.); - } -} // btSliderConstraint::testLinLimits() - -//----------------------------------------------------------------------------- - -void btSliderConstraint::testAngLimits(void) -{ - m_angDepth = btScalar(0.); - m_solveAngLim = false; - if(m_lowerAngLimit <= m_upperAngLimit) - { - const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); - const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); - const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); - btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); - m_angPos = rot; - if(rot < m_lowerAngLimit) - { - m_angDepth = rot - m_lowerAngLimit; - m_solveAngLim = true; - } - else if(rot > m_upperAngLimit) - { - m_angDepth = rot - m_upperAngLimit; - m_solveAngLim = true; - } - } -} // btSliderConstraint::testAngLimits() - -//----------------------------------------------------------------------------- - -btVector3 btSliderConstraint::getAncorInA(void) -{ - btVector3 ancorInA; - ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; - ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; - return ancorInA; -} // btSliderConstraint::getAncorInA() - -//----------------------------------------------------------------------------- - -btVector3 btSliderConstraint::getAncorInB(void) -{ - btVector3 ancorInB; - ancorInB = m_frameInB.getOrigin(); - return ancorInB; -} // btSliderConstraint::getAncorInB(); +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 +*/ + +//----------------------------------------------------------------------------- + +#include "btSliderConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include + +//----------------------------------------------------------------------------- + +void btSliderConstraint::initParams() +{ + m_lowerLinLimit = btScalar(1.0); + m_upperLinLimit = btScalar(-1.0); + m_lowerAngLimit = btScalar(0.); + m_upperAngLimit = btScalar(0.); + m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirLin = btScalar(0.); + m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingDirAng = btScalar(0.); + m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING; + m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS; + m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION; + m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING; + + m_poweredLinMotor = false; + m_targetLinMotorVelocity = btScalar(0.); + m_maxLinMotorForce = btScalar(0.); + m_accumulatedLinMotorImpulse = btScalar(0.0); + + m_poweredAngMotor = false; + m_targetAngMotorVelocity = btScalar(0.); + m_maxAngMotorForce = btScalar(0.); + m_accumulatedAngMotorImpulse = btScalar(0.0); + +} // btSliderConstraint::initParams() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint() + :btTypedConstraint(SLIDER_CONSTRAINT_TYPE), + m_useLinearReferenceFrameA(true), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA) + : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB) + , m_frameInA(frameInA) + , m_frameInB(frameInB), + m_useLinearReferenceFrameA(useLinearReferenceFrameA), + m_useSolveConstraintObsolete(false) +// m_useSolveConstraintObsolete(true) +{ + initParams(); +} // btSliderConstraint::btSliderConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobian() +{ + if (!m_useSolveConstraintObsolete) + { + return; + } + if(m_useLinearReferenceFrameA) + { + buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); + } + else + { + buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); + } +} // btSliderConstraint::buildJacobian() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +{ + //calculate transforms + m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; + m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + m_delta = m_realPivotBInW - m_realPivotAInW; + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); + m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacLin[i]) btJacobianEntry( + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + m_relPosA, + m_relPosB, + normalWorld, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass() + ); + m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); + m_depth[i] = m_delta.dot(normalWorld); + } + testLinLimits(); + // angular part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + new (&m_jacAng[i]) btJacobianEntry( + normalWorld, + rbA.getCenterOfMassTransform().getBasis().transpose(), + rbB.getCenterOfMassTransform().getBasis().transpose(), + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal() + ); + } + testAngLimits(); + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); + // clear accumulator for motors + m_accumulatedLinMotorImpulse = btScalar(0.0); + m_accumulatedAngMotorImpulse = btScalar(0.0); +} // btSliderConstraint::buildJacobianInt() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::getInfo1(btConstraintInfo1* info) +{ + if (m_useSolveConstraintObsolete) + { + info->m_numConstraintRows = 0; + info->nub = 0; + } + else + { + info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular + info->nub = 2; + //prepare constraint + calculateTransforms(); + testLinLimits(); + if(getSolveLinLimit() || getPoweredLinMotor()) + { + info->m_numConstraintRows++; // limit 3rd linear as well + info->nub--; + } + testAngLimits(); + if(getSolveAngLimit() || getPoweredAngMotor()) + { + info->m_numConstraintRows++; // limit 3rd angular as well + info->nub--; + } + } +} // btSliderConstraint::getInfo1() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::getInfo2(btConstraintInfo2* info) +{ + btAssert(!m_useSolveConstraintObsolete); + int i, s = info->rowskip; + const btTransform& trA = getCalculatedTransformA(); + const btTransform& trB = getCalculatedTransformB(); + btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f); + // make rotations around Y and Z equal + // the slider axis should be the only unconstrained + // rotational axis, the angular velocity of the two bodies perpendicular to + // the slider axis should be equal. thus the constraint equations are + // p*w1 - p*w2 = 0 + // q*w1 - q*w2 = 0 + // where p and q are unit vectors normal to the slider axis, and w1 and w2 + // are the angular velocity vectors of the two bodies. + // get slider axis (X) + btVector3 ax1 = trA.getBasis().getColumn(0); + // get 2 orthos to slider axis (Y, Z) + btVector3 p = trA.getBasis().getColumn(1); + btVector3 q = trA.getBasis().getColumn(2); + // set the two slider rows + info->m_J1angularAxis[0] = p[0]; + info->m_J1angularAxis[1] = p[1]; + info->m_J1angularAxis[2] = p[2]; + info->m_J1angularAxis[s+0] = q[0]; + info->m_J1angularAxis[s+1] = q[1]; + info->m_J1angularAxis[s+2] = q[2]; + + info->m_J2angularAxis[0] = -p[0]; + info->m_J2angularAxis[1] = -p[1]; + info->m_J2angularAxis[2] = -p[2]; + info->m_J2angularAxis[s+0] = -q[0]; + info->m_J2angularAxis[s+1] = -q[1]; + info->m_J2angularAxis[s+2] = -q[2]; + // compute the right hand side of the constraint equation. set relative + // body velocities along p and q to bring the slider back into alignment. + // if ax1,ax2 are the unit length slider axes as computed from body1 and + // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). + // if "theta" is the angle between ax1 and ax2, we need an angular velocity + // along u to cover angle erp*theta in one step : + // |angular_velocity| = angle/time = erp*theta / stepsize + // = (erp*fps) * theta + // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| + // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) + // ...as ax1 and ax2 are unit length. if theta is smallish, + // theta ~= sin(theta), so + // angular_velocity = (erp*fps) * (ax1 x ax2) + // ax1 x ax2 is in the plane space of ax1, so we project the angular + // velocity to p and q to find the right hand side. + btScalar k = info->fps * info->erp * getSoftnessOrthoAng(); + btVector3 ax2 = trB.getBasis().getColumn(0); + btVector3 u = ax1.cross(ax2); + info->m_constraintError[0] = k * u.dot(p); + info->m_constraintError[s] = k * u.dot(q); + // pull out pos and R for both bodies. also get the connection + // vector c = pos2-pos1. + // next two rows. we want: vel2 = vel1 + w1 x c ... but this would + // result in three equations, so we project along the planespace vectors + // so that sliding along the slider axis is disregarded. for symmetry we + // also consider rotation around center of mass of two bodies (factA and factB). + btTransform bodyA_trans = m_rbA.getCenterOfMassTransform(); + btTransform bodyB_trans = m_rbB.getCenterOfMassTransform(); + int s2 = 2 * s, s3 = 3 * s; + btVector3 c; + btScalar miA = m_rbA.getInvMass(); + btScalar miB = m_rbB.getInvMass(); + btScalar miS = miA + miB; + btScalar factA, factB; + if(miS > btScalar(0.f)) + { + factA = miB / miS; + } + else + { + factA = btScalar(0.5f); + } + if(factA > 0.99f) factA = 0.99f; + if(factA < 0.01f) factA = 0.01f; + factB = btScalar(1.0f) - factA; + c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); + btVector3 tmp = c.cross(p); + for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i]; + tmp = c.cross(q); + for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i]; + for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i]; + + for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; + for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + // compute two elements of right hand side. we want to align the offset + // point (in body 2's frame) with the center of body 1. + btVector3 ofs; // offset point in global coordinates + ofs = trB.getOrigin() - trA.getOrigin(); + k = info->fps * info->erp * getSoftnessOrthoLin(); + info->m_constraintError[s2] = k * p.dot(ofs); + info->m_constraintError[s3] = k * q.dot(ofs); + int nrow = 3; // last filled row + int srow; + // check linear limits linear + btScalar limit_err = btScalar(0.0); + int limit = 0; + if(getSolveLinLimit()) + { + limit_err = getLinDepth() * signFact; + limit = (limit_err > btScalar(0.0)) ? 2 : 1; + } + int powered = 0; + if(getPoweredLinMotor()) + { + powered = 1; + } + // if the slider has joint limits or motor, add in the extra row + if (limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1linearAxis[srow+0] = ax1[0]; + info->m_J1linearAxis[srow+1] = ax1[1]; + info->m_J1linearAxis[srow+2] = ax1[2]; + // linear torque decoupling step: + // + // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies + // do not create a torque couple. in other words, the points that the + // constraint force is applied at must lie along the same ax1 axis. + // a torque couple will result in limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the center of mass of the two bodies + btVector3 ltd; // Linear Torque Decoupling vector (a torque) +// c = btScalar(0.5) * c; + ltd = c.cross(ax1); + info->m_J1angularAxis[srow+0] = factA*ltd[0]; + info->m_J1angularAxis[srow+1] = factA*ltd[1]; + info->m_J1angularAxis[srow+2] = factA*ltd[2]; + info->m_J2angularAxis[srow+0] = factB*ltd[0]; + info->m_J2angularAxis[srow+1] = factB*ltd[1]; + info->m_J2angularAxis[srow+2] = factB*ltd[2]; + // right-hand part + btScalar lostop = getLowerLinLimit(); + btScalar histop = getUpperLinLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + info->m_constraintError[srow] = 0.; + info->m_lowerLimit[srow] = 0.; + info->m_upperLimit[srow] = 0.; + if(powered) + { + info->cfm[nrow] = btScalar(0.0); + btScalar tag_vel = getTargetLinMotorVelocity(); + btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp); +// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity(); + info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity(); + info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps; + info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + else + { // high limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getLinearVelocity().dot(ax1); + vel -= m_rbB.getLinearVelocity().dot(ax1); + vel *= signFact; + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if (newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimLin(); + } // if(limit) + } // if linear limit + // check angular limits + limit_err = btScalar(0.0); + limit = 0; + if(getSolveAngLimit()) + { + limit_err = getAngDepth(); + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + // if the slider has joint limits, add in the extra row + powered = 0; + if(getPoweredAngMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->m_J1angularAxis[srow+0] = ax1[0]; + info->m_J1angularAxis[srow+1] = ax1[1]; + info->m_J1angularAxis[srow+2] = ax1[2]; + + info->m_J2angularAxis[srow+0] = -ax1[0]; + info->m_J2angularAxis[srow+1] = -ax1[1]; + info->m_J2angularAxis[srow+2] = -ax1[2]; + + btScalar lostop = getLowerAngLimit(); + btScalar histop = getUpperAngLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + if(powered) + { + info->cfm[srow] = btScalar(0.0); + btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp); + info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity(); + info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps; + info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps; + } + if(limit) + { + k = info->fps * info->erp; + info->m_constraintError[srow] += k * limit_err; + info->cfm[srow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { + // limited low and high simultaneously + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else if(limit == 1) + { // low limit + info->m_lowerLimit[srow] = 0; + info->m_upperLimit[srow] = SIMD_INFINITY; + } + else + { // high limit + info->m_lowerLimit[srow] = -SIMD_INFINITY; + info->m_upperLimit[srow] = 0; + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_rbA.getAngularVelocity().dot(ax1); + vel -= m_rbB.getAngularVelocity().dot(ax1); + // only apply bounce if the velocity is incoming, and if the + // resulting c[] exceeds what we already have. + if(limit == 1) + { // low limit + if(vel < 0) + { + btScalar newc = -bounce * vel; + if(newc > info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + else + { // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->m_constraintError[srow]) + { + info->m_constraintError[srow] = newc; + } + } + } + } + info->m_constraintError[srow] *= getSoftnessLimAng(); + } // if(limit) + } // if angular limit or powered +} // btSliderConstraint::getInfo2() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +{ + if (m_useSolveConstraintObsolete) + { + m_timeStep = timeStep; + if(m_useLinearReferenceFrameA) + { + solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); + } + else + { + solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); + } + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) +{ + int i; + // linear + btVector3 velA; + bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); + btVector3 velB; + bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); + btVector3 vel = velA - velB; + for(i = 0; i < 3; i++) + { + const btVector3& normal = m_jacLin[i].m_linearJointAxis; + btScalar rel_vel = normal.dot(vel); + // calculate positional error + btScalar depth = m_depth[i]; + // get parameters + btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); + btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); + btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); + // calcutate and apply impulse + btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; + btVector3 impulse_vector = normal * normalImpulse; + + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + + if(m_poweredLinMotor && (!i)) + { // apply linear motor + if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) + { + btScalar desiredMotorVel = m_targetLinMotorVelocity; + btScalar motor_relvel = desiredMotorVel + rel_vel; + normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); + if(new_acc > m_maxLinMotorForce) + { + new_acc = m_maxLinMotorForce; + } + btScalar del = new_acc - m_accumulatedLinMotorImpulse; + if(normalImpulse < btScalar(0.0)) + { + normalImpulse = -del; + } + else + { + normalImpulse = del; + } + m_accumulatedLinMotorImpulse = new_acc; + // apply clamped impulse + impulse_vector = normal * normalImpulse; + //rbA.applyImpulse( impulse_vector, m_relPosA); + //rbB.applyImpulse(-impulse_vector, m_relPosB); + + { + btVector3 ftorqueAxis1 = m_relPosA.cross(normal); + btVector3 ftorqueAxis2 = m_relPosB.cross(normal); + bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + } + + + + } + } + } + // angular + // get axes in world space + btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); + btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); + + btVector3 angVelA; + bodyA.getAngularVelocity(angVelA); + btVector3 angVelB; + bodyB.getAngularVelocity(angVelB); + + btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); + btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); + + btVector3 angAorthog = angVelA - angVelAroundAxisA; + btVector3 angBorthog = angVelB - angVelAroundAxisB; + btVector3 velrelOrthog = angAorthog-angBorthog; + //solve orthogonal angular velocity correction + btScalar len = velrelOrthog.length(); + btScalar orthorImpulseMag = 0.f; + + if (len > btScalar(0.00001)) + { + btVector3 normal = velrelOrthog.normalized(); + btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); + //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; + } + //solve angular positional correction + btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); + btVector3 angularAxis = angularError; + btScalar angularImpulseMag = 0; + + btScalar len2 = angularError.length(); + if (len2>btScalar(0.00001)) + { + btVector3 normal2 = angularError.normalized(); + btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); + angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; + angularError *= angularImpulseMag; + } + // apply impulse + //rbA.applyTorqueImpulse(-velrelOrthog+angularError); + //rbB.applyTorqueImpulse(velrelOrthog-angularError); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); + + + btScalar impulseMag; + //solve angular limits + if(m_solveAngLim) + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessLimAng; + } + else + { + impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; + impulseMag *= m_kAngle * m_softnessDirAng; + } + btVector3 impulse = axisA * impulseMag; + //rbA.applyTorqueImpulse(impulse); + //rbB.applyTorqueImpulse(-impulse); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); + + + + //apply angular motor + if(m_poweredAngMotor) + { + if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) + { + btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; + btScalar projRelVel = velrel.dot(axisA); + + btScalar desiredMotorVel = m_targetAngMotorVelocity; + btScalar motor_relvel = desiredMotorVel - projRelVel; + + btScalar angImpulse = m_kAngle * motor_relvel; + // clamp accumulated impulse + btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); + if(new_acc > m_maxAngMotorForce) + { + new_acc = m_maxAngMotorForce; + } + btScalar del = new_acc - m_accumulatedAngMotorImpulse; + if(angImpulse < btScalar(0.0)) + { + angImpulse = -del; + } + else + { + angImpulse = del; + } + m_accumulatedAngMotorImpulse = new_acc; + // apply clamped impulse + btVector3 motorImp = angImpulse * axisA; + //rbA.applyTorqueImpulse(motorImp); + //rbB.applyTorqueImpulse(-motorImp); + + bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); + bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); + } + } +} // btSliderConstraint::solveConstraint() + +//----------------------------------------------------------------------------- + +//----------------------------------------------------------------------------- + +void btSliderConstraint::calculateTransforms(void){ + if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete)) + { + m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA; + m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB; + } + else + { + m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB; + m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA; + } + m_realPivotAInW = m_calculatedTransformA.getOrigin(); + m_realPivotBInW = m_calculatedTransformB.getOrigin(); + m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X + if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete) + { + m_delta = m_realPivotBInW - m_realPivotAInW; + } + else + { + m_delta = m_realPivotAInW - m_realPivotBInW; + } + m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; + btVector3 normalWorld; + int i; + //linear part + for(i = 0; i < 3; i++) + { + normalWorld = m_calculatedTransformA.getBasis().getColumn(i); + m_depth[i] = m_delta.dot(normalWorld); + } +} // btSliderConstraint::calculateTransforms() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::testLinLimits(void) +{ + m_solveLinLim = false; + m_linPos = m_depth[0]; + if(m_lowerLinLimit <= m_upperLinLimit) + { + if(m_depth[0] > m_upperLinLimit) + { + m_depth[0] -= m_upperLinLimit; + m_solveLinLim = true; + } + else if(m_depth[0] < m_lowerLinLimit) + { + m_depth[0] -= m_lowerLinLimit; + m_solveLinLim = true; + } + else + { + m_depth[0] = btScalar(0.); + } + } + else + { + m_depth[0] = btScalar(0.); + } +} // btSliderConstraint::testLinLimits() + +//----------------------------------------------------------------------------- + +void btSliderConstraint::testAngLimits(void) +{ + m_angDepth = btScalar(0.); + m_solveAngLim = false; + if(m_lowerAngLimit <= m_upperAngLimit) + { + const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1); + const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2); + const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1); + btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); + m_angPos = rot; + if(rot < m_lowerAngLimit) + { + m_angDepth = rot - m_lowerAngLimit; + m_solveAngLim = true; + } + else if(rot > m_upperAngLimit) + { + m_angDepth = rot - m_upperAngLimit; + m_solveAngLim = true; + } + } +} // btSliderConstraint::testAngLimits() + +//----------------------------------------------------------------------------- + +btVector3 btSliderConstraint::getAncorInA(void) +{ + btVector3 ancorInA; + ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis; + ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA; + return ancorInA; +} // btSliderConstraint::getAncorInA() + +//----------------------------------------------------------------------------- + +btVector3 btSliderConstraint::getAncorInB(void) +{ + btVector3 ancorInB; + ancorInB = m_frameInB.getOrigin(); + return ancorInB; +} // btSliderConstraint::getAncorInB(); diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 9357b8fa3..70fbce5d9 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -1,229 +1,229 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -Added by Roman Ponomarev (rponom@gmail.com) -April 04, 2008 - -TODO: - - add clamping od accumulated impulse to improve stability - - add conversion for ODE constraint solver -*/ - -#ifndef SLIDER_CONSTRAINT_H -#define SLIDER_CONSTRAINT_H - -//----------------------------------------------------------------------------- - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -//----------------------------------------------------------------------------- - -class btRigidBody; - -//----------------------------------------------------------------------------- - -#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) -#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) - -//----------------------------------------------------------------------------- - -class btSliderConstraint : public btTypedConstraint -{ -protected: - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - btTransform m_frameInA; - btTransform m_frameInB; - // use frameA fo define limits, if true - bool m_useLinearReferenceFrameA; - // linear limits - btScalar m_lowerLinLimit; - btScalar m_upperLinLimit; - // angular limits - btScalar m_lowerAngLimit; - btScalar m_upperAngLimit; - // softness, restitution and damping for different cases - // DirLin - moving inside linear limits - // LimLin - hitting linear limit - // DirAng - moving inside angular limits - // LimAng - hitting angular limit - // OrthoLin, OrthoAng - against constraint axis - btScalar m_softnessDirLin; - btScalar m_restitutionDirLin; - btScalar m_dampingDirLin; - btScalar m_softnessDirAng; - btScalar m_restitutionDirAng; - btScalar m_dampingDirAng; - btScalar m_softnessLimLin; - btScalar m_restitutionLimLin; - btScalar m_dampingLimLin; - btScalar m_softnessLimAng; - btScalar m_restitutionLimAng; - btScalar m_dampingLimAng; - btScalar m_softnessOrthoLin; - btScalar m_restitutionOrthoLin; - btScalar m_dampingOrthoLin; - btScalar m_softnessOrthoAng; - btScalar m_restitutionOrthoAng; - btScalar m_dampingOrthoAng; - - // for interlal use - bool m_solveLinLim; - bool m_solveAngLim; - - btJacobianEntry m_jacLin[3]; - btScalar m_jacLinDiagABInv[3]; - - btJacobianEntry m_jacAng[3]; - - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - - btVector3 m_sliderAxis; - btVector3 m_realPivotAInW; - btVector3 m_realPivotBInW; - btVector3 m_projPivotInW; - btVector3 m_delta; - btVector3 m_depth; - btVector3 m_relPosA; - btVector3 m_relPosB; - - btScalar m_linPos; - btScalar m_angPos; - - btScalar m_angDepth; - btScalar m_kAngle; - - bool m_poweredLinMotor; - btScalar m_targetLinMotorVelocity; - btScalar m_maxLinMotorForce; - btScalar m_accumulatedLinMotorImpulse; - - bool m_poweredAngMotor; - btScalar m_targetAngMotorVelocity; - btScalar m_maxAngMotorForce; - btScalar m_accumulatedAngMotorImpulse; - - //------------------------ - void initParams(); -public: - // constructors - btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btSliderConstraint(); - // overrides - virtual void buildJacobian(); - virtual void getInfo1 (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - - - // access - const btRigidBody& getRigidBodyA() const { return m_rbA; } - const btRigidBody& getRigidBodyB() const { return m_rbB; } - const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } - const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } - const btTransform & getFrameOffsetA() const { return m_frameInA; } - const btTransform & getFrameOffsetB() const { return m_frameInB; } - btTransform & getFrameOffsetA() { return m_frameInA; } - btTransform & getFrameOffsetB() { return m_frameInB; } - btScalar getLowerLinLimit() { return m_lowerLinLimit; } - void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } - btScalar getUpperLinLimit() { return m_upperLinLimit; } - void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } - btScalar getLowerAngLimit() { return m_lowerAngLimit; } - void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } - btScalar getUpperAngLimit() { return m_upperAngLimit; } - void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } - bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } - btScalar getSoftnessDirLin() { return m_softnessDirLin; } - btScalar getRestitutionDirLin() { return m_restitutionDirLin; } - btScalar getDampingDirLin() { return m_dampingDirLin ; } - btScalar getSoftnessDirAng() { return m_softnessDirAng; } - btScalar getRestitutionDirAng() { return m_restitutionDirAng; } - btScalar getDampingDirAng() { return m_dampingDirAng; } - btScalar getSoftnessLimLin() { return m_softnessLimLin; } - btScalar getRestitutionLimLin() { return m_restitutionLimLin; } - btScalar getDampingLimLin() { return m_dampingLimLin; } - btScalar getSoftnessLimAng() { return m_softnessLimAng; } - btScalar getRestitutionLimAng() { return m_restitutionLimAng; } - btScalar getDampingLimAng() { return m_dampingLimAng; } - btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } - btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } - btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } - btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } - btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } - btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } - void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } - void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } - void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } - void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } - void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } - void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } - void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } - void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } - void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } - void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } - void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } - void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } - void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } - void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } - void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } - void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } - void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } - void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } - void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } - bool getPoweredLinMotor() { return m_poweredLinMotor; } - void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } - btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } - void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } - btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } - void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } - bool getPoweredAngMotor() { return m_poweredAngMotor; } - void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } - btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } - void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } - btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - btScalar getLinearPos() { return m_linPos; } - - - // access for ODE solver - bool getSolveLinLimit() { return m_solveLinLim; } - btScalar getLinDepth() { return m_depth[0]; } - bool getSolveAngLimit() { return m_solveAngLim; } - btScalar getAngDepth() { return m_angDepth; } - // internal - void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); - // shared code used by ODE solver - void calculateTransforms(void); - void testLinLimits(void); - void testLinLimits2(btConstraintInfo2* info); - void testAngLimits(void); - // access for PE Solver - btVector3 getAncorInA(void); - btVector3 getAncorInB(void); -}; - -//----------------------------------------------------------------------------- - -#endif //SLIDER_CONSTRAINT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +Added by Roman Ponomarev (rponom@gmail.com) +April 04, 2008 + +TODO: + - add clamping od accumulated impulse to improve stability + - add conversion for ODE constraint solver +*/ + +#ifndef SLIDER_CONSTRAINT_H +#define SLIDER_CONSTRAINT_H + +//----------------------------------------------------------------------------- + +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +//----------------------------------------------------------------------------- + +class btRigidBody; + +//----------------------------------------------------------------------------- + +#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) +#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) + +//----------------------------------------------------------------------------- + +class btSliderConstraint : public btTypedConstraint +{ +protected: + ///for backwards compatibility during the transition to 'getInfo/getInfo2' + bool m_useSolveConstraintObsolete; + btTransform m_frameInA; + btTransform m_frameInB; + // use frameA fo define limits, if true + bool m_useLinearReferenceFrameA; + // linear limits + btScalar m_lowerLinLimit; + btScalar m_upperLinLimit; + // angular limits + btScalar m_lowerAngLimit; + btScalar m_upperAngLimit; + // softness, restitution and damping for different cases + // DirLin - moving inside linear limits + // LimLin - hitting linear limit + // DirAng - moving inside angular limits + // LimAng - hitting angular limit + // OrthoLin, OrthoAng - against constraint axis + btScalar m_softnessDirLin; + btScalar m_restitutionDirLin; + btScalar m_dampingDirLin; + btScalar m_softnessDirAng; + btScalar m_restitutionDirAng; + btScalar m_dampingDirAng; + btScalar m_softnessLimLin; + btScalar m_restitutionLimLin; + btScalar m_dampingLimLin; + btScalar m_softnessLimAng; + btScalar m_restitutionLimAng; + btScalar m_dampingLimAng; + btScalar m_softnessOrthoLin; + btScalar m_restitutionOrthoLin; + btScalar m_dampingOrthoLin; + btScalar m_softnessOrthoAng; + btScalar m_restitutionOrthoAng; + btScalar m_dampingOrthoAng; + + // for interlal use + bool m_solveLinLim; + bool m_solveAngLim; + + btJacobianEntry m_jacLin[3]; + btScalar m_jacLinDiagABInv[3]; + + btJacobianEntry m_jacAng[3]; + + btScalar m_timeStep; + btTransform m_calculatedTransformA; + btTransform m_calculatedTransformB; + + btVector3 m_sliderAxis; + btVector3 m_realPivotAInW; + btVector3 m_realPivotBInW; + btVector3 m_projPivotInW; + btVector3 m_delta; + btVector3 m_depth; + btVector3 m_relPosA; + btVector3 m_relPosB; + + btScalar m_linPos; + btScalar m_angPos; + + btScalar m_angDepth; + btScalar m_kAngle; + + bool m_poweredLinMotor; + btScalar m_targetLinMotorVelocity; + btScalar m_maxLinMotorForce; + btScalar m_accumulatedLinMotorImpulse; + + bool m_poweredAngMotor; + btScalar m_targetAngMotorVelocity; + btScalar m_maxAngMotorForce; + btScalar m_accumulatedAngMotorImpulse; + + //------------------------ + void initParams(); +public: + // constructors + btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btSliderConstraint(); + // overrides + virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); + + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + + + // access + const btRigidBody& getRigidBodyA() const { return m_rbA; } + const btRigidBody& getRigidBodyB() const { return m_rbB; } + const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } + const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } + const btTransform & getFrameOffsetA() const { return m_frameInA; } + const btTransform & getFrameOffsetB() const { return m_frameInB; } + btTransform & getFrameOffsetA() { return m_frameInA; } + btTransform & getFrameOffsetB() { return m_frameInB; } + btScalar getLowerLinLimit() { return m_lowerLinLimit; } + void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } + btScalar getUpperLinLimit() { return m_upperLinLimit; } + void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } + btScalar getLowerAngLimit() { return m_lowerAngLimit; } + void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } + btScalar getUpperAngLimit() { return m_upperAngLimit; } + void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } + bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } + btScalar getSoftnessDirLin() { return m_softnessDirLin; } + btScalar getRestitutionDirLin() { return m_restitutionDirLin; } + btScalar getDampingDirLin() { return m_dampingDirLin ; } + btScalar getSoftnessDirAng() { return m_softnessDirAng; } + btScalar getRestitutionDirAng() { return m_restitutionDirAng; } + btScalar getDampingDirAng() { return m_dampingDirAng; } + btScalar getSoftnessLimLin() { return m_softnessLimLin; } + btScalar getRestitutionLimLin() { return m_restitutionLimLin; } + btScalar getDampingLimLin() { return m_dampingLimLin; } + btScalar getSoftnessLimAng() { return m_softnessLimAng; } + btScalar getRestitutionLimAng() { return m_restitutionLimAng; } + btScalar getDampingLimAng() { return m_dampingLimAng; } + btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } + btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } + btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } + btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } + btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } + btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } + void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } + void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } + void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } + void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } + void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } + void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } + void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } + void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } + void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } + void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } + void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } + void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } + void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } + void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } + void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } + void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } + void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } + void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } + void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } + bool getPoweredLinMotor() { return m_poweredLinMotor; } + void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } + btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } + void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } + btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } + void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } + bool getPoweredAngMotor() { return m_poweredAngMotor; } + void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } + btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } + void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } + btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } + btScalar getLinearPos() { return m_linPos; } + + + // access for ODE solver + bool getSolveLinLimit() { return m_solveLinLim; } + btScalar getLinDepth() { return m_depth[0]; } + bool getSolveAngLimit() { return m_solveAngLim; } + btScalar getAngDepth() { return m_angDepth; } + // internal + void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); + void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); + // shared code used by ODE solver + void calculateTransforms(void); + void testLinLimits(void); + void testLinLimits2(btConstraintInfo2* info); + void testAngLimits(void); + // access for PE Solver + btVector3 getAncorInA(void); + btVector3 getAncorInB(void); +}; + +//----------------------------------------------------------------------------- + +#endif //SLIDER_CONSTRAINT_H + diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index e22eb4c2b..98c9876ae 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -1,179 +1,179 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOLVER_BODY_H -#define BT_SOLVER_BODY_H - -class btRigidBody; -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btTransformUtil.h" - -///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision -#ifdef BT_USE_SSE -#define USE_SIMD 1 -#endif // - - -#ifdef USE_SIMD - -struct btSimdScalar -{ - SIMD_FORCE_INLINE btSimdScalar() - { - - } - - SIMD_FORCE_INLINE btSimdScalar(float fl) - :m_vec128 (_mm_set1_ps(fl)) - { - } - - SIMD_FORCE_INLINE btSimdScalar(__m128 v128) - :m_vec128(v128) - { - } - union - { - __m128 m_vec128; - float m_floats[4]; - int m_ints[4]; - btScalar m_unusedPadding; - }; - SIMD_FORCE_INLINE __m128 get128() - { - return m_vec128; - } - - SIMD_FORCE_INLINE const __m128 get128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE void set128(__m128 v128) - { - m_vec128 = v128; - } - - SIMD_FORCE_INLINE operator __m128() - { - return m_vec128; - } - SIMD_FORCE_INLINE operator const __m128() const - { - return m_vec128; - } - - SIMD_FORCE_INLINE operator float() const - { - return m_floats[0]; - } - -}; - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator*(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); -} - -///@brief Return the elementwise product of two btSimdScalar -SIMD_FORCE_INLINE btSimdScalar -operator+(const btSimdScalar& v1, const btSimdScalar& v2) -{ - return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); -} - - -#else -#define btSimdScalar btScalar -#endif - -///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - btVector3 m_deltaLinearVelocity; - btVector3 m_deltaAngularVelocity; - btScalar m_angularFactor; - btScalar m_invMass; - btScalar m_friction; - btRigidBody* m_originalBody; - btVector3 m_pushVelocity; - //btVector3 m_turnVelocity; - - - SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const - { - if (m_originalBody) - velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); - else - velocity.setValue(0,0,0); - } - - SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const - { - if (m_originalBody) - angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; - else - angVel.setValue(0,0,0); - } - - - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) - { - //if (m_invMass) - { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; - m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } - } - - -/* - - void writebackVelocity() - { - if (m_invMass) - { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); - - //m_originalBody->setCompanionId(-1); - } - } - */ - - void writebackVelocity(btScalar timeStep=0) - { - if (m_invMass) - { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); - //m_originalBody->setCompanionId(-1); - } - } - - - -}; - -#endif //BT_SOLVER_BODY_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btAlignedAllocator.h" +#include "LinearMath/btTransformUtil.h" + +///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision +#ifdef BT_USE_SSE +#define USE_SIMD 1 +#endif // + + +#ifdef USE_SIMD + +struct btSimdScalar +{ + SIMD_FORCE_INLINE btSimdScalar() + { + + } + + SIMD_FORCE_INLINE btSimdScalar(float fl) + :m_vec128 (_mm_set1_ps(fl)) + { + } + + SIMD_FORCE_INLINE btSimdScalar(__m128 v128) + :m_vec128(v128) + { + } + union + { + __m128 m_vec128; + float m_floats[4]; + int m_ints[4]; + btScalar m_unusedPadding; + }; + SIMD_FORCE_INLINE __m128 get128() + { + return m_vec128; + } + + SIMD_FORCE_INLINE const __m128 get128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE void set128(__m128 v128) + { + m_vec128 = v128; + } + + SIMD_FORCE_INLINE operator __m128() + { + return m_vec128; + } + SIMD_FORCE_INLINE operator const __m128() const + { + return m_vec128; + } + + SIMD_FORCE_INLINE operator float() const + { + return m_floats[0]; + } + +}; + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator*(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); +} + +///@brief Return the elementwise product of two btSimdScalar +SIMD_FORCE_INLINE btSimdScalar +operator+(const btSimdScalar& v1, const btSimdScalar& v2) +{ + return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); +} + + +#else +#define btSimdScalar btScalar +#endif + +///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. +ATTRIBUTE_ALIGNED16 (struct) btSolverBody +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btScalar m_angularFactor; + btScalar m_invMass; + btScalar m_friction; + btRigidBody* m_originalBody; + btVector3 m_pushVelocity; + //btVector3 m_turnVelocity; + + + SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + { + if (m_originalBody) + velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + else + velocity.setValue(0,0,0); + } + + SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const + { + if (m_originalBody) + angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + { + //if (m_invMass) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + +/* + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + + //m_originalBody->setCompanionId(-1); + } + } + */ + + void writebackVelocity(btScalar timeStep=0) + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); + m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + //m_originalBody->setCompanionId(-1); + } + } + + + +}; + +#endif //BT_SOLVER_BODY_H + + diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index c8cf0bb80..3fa98ee4c 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: static btRigidBody s_fixed(0, 0,0); -#define DEFAULT_DEBUGDRAW_SIZE btScalar(5.f) +#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) btTypedConstraint::btTypedConstraint(btTypedConstraintType type) :m_userConstraintType(-1), diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 2d79eb89c..74a3a6584 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1,1428 +1,1428 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btDiscreteDynamicsWorld.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include "LinearMath/btTransformUtil.h" -#include "LinearMath/btQuickprof.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" -#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" - -//for debug rendering -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -#include "LinearMath/btIDebugDraw.h" - - - -//vehicle -#include "BulletDynamics/Vehicle/btRaycastVehicle.h" -#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" -#include "BulletDynamics/Vehicle/btWheelInfo.h" -//character -#include "BulletDynamics/Character/btCharacterControllerInterface.h" - -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btMotionState.h" - - - - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_gravity(0,-10,0), -m_localTime(btScalar(1.)/btScalar(60.)), -m_profileTimings(0) -{ - if (!m_constraintSolver) - { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; - m_ownsConstraintSolver = true; - } else - { - m_ownsConstraintSolver = false; - } - - { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); - m_islandManager = new (mem) btSimulationIslandManager(); - } - - m_ownsIslandManager = true; -} - - -btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() -{ - //only delete it when we created it - if (m_ownsIslandManager) - { - m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); - } - if (m_ownsConstraintSolver) - { - - m_constraintSolver->~btConstraintSolver(); - btAlignedFree(m_constraintSolver); - } -} - -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) -{ - - for (int i=0;igetActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - } -} - -void btDiscreteDynamicsWorld::debugDrawWorld() -{ - BT_PROFILE("debugDrawWorld"); - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) - { - int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(0,0,0); - for (int i=0;igetManifoldByIndexInternal(i); - //btCollisionObject* obA = static_cast(contactManifold->getBody0()); - //btCollisionObject* obB = static_cast(contactManifold->getBody1()); - - int numContacts = contactManifold->getNumContacts(); - for (int j=0;jgetContactPoint(j); - getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); - } - } - } - bool drawConstraints = false; - if (getDebugDrawer()) - { - int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) - { - drawConstraints = true; - } - } - if(drawConstraints) - { - for(int i = getNumConstraints()-1; i>=0 ;i--) - { - btTypedConstraint* constraint = getConstraint(i); - debugDrawConstraint(constraint); - } - } - - - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) - { - int i; - - for ( i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); - switch(colObj->getActivationState()) - { - case ACTIVE_TAG: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; - case ISLAND_SLEEPING: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; - case WANTS_DEACTIVATION: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; - case DISABLE_DEACTIVATION: - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; - case DISABLE_SIMULATION: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; - default: - { - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); - } - }; - - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - btVector3 minAabb,maxAabb; - btVector3 colorvec(1,0,0); - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - - } - - for ( i=0;im_vehicles.size();i++) - { - for (int v=0;vgetNumWheels();v++) - { - btVector3 wheelColor(0,255,255); - if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,255); - } else - { - wheelColor.setValue(255,0,255); - } - - btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); - - btVector3 axle = btVector3( - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); - - - //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS - //debug wheels (cylinders) - m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - - } - } - } -} - -void btDiscreteDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iclearForces(); - } - } -} - -///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iisActive()) - { - body->applyGravity(); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) -{ - btAssert(body); - - if (body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - ///@todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } -} - - -void btDiscreteDynamicsWorld::synchronizeMotionStates() -{ - BT_PROFILE("synchronizeMotionStates"); - { - //todo: iterate over awake simulation islands! - for ( int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - for ( int i=0;im_vehicles.size();i++) - { - for (int v=0;vgetNumWheels();v++) - { - //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicles[i]->updateWheelTransform(v,true); - } - } - } - -} - - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - startProfiling(timeStep); - - BT_PROFILE("stepSimulation"); - - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = timeStep; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - btIDebugDraw* debugDrawer = getDebugDrawer (); - gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - - saveKinematicState(fixedTimeStep); - - applyGravity(); - - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; - - for (int i=0;isetGravity(gravity); - } - } -} - -btVector3 btDiscreteDynamicsWorld::getGravity () const -{ - return m_gravity; -} - - -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - removeCollisionObject(body); -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) -{ - if (!body->isStaticOrKinematicObject()) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); - } -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) -{ - if (!body->isStaticOrKinematicObject()) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - addCollisionObject(body,group,mask); - } -} - - -void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) -{ - BT_PROFILE("updateVehicles"); - - for ( int i=0;iupdateVehicle( timeStep); - } -} - -void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) -{ - BT_PROFILE("updateCharacters"); - - for ( int i=0;ipreStep (this); - character->playerStep (this,timeStep); - } -} - - - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BT_PROFILE("updateActivationState"); - - for ( int i=0;iupdateDeactivation(timeStep); - - if (body->wantsSleeping()) - { - if (body->isStaticOrKinematicObject()) - { - body->setActivationState(ISLAND_SLEEPING); - } else - { - if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); - if (body->getActivationState() == ISLAND_SLEEPING) - { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); - } - - } - } else - { - if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); - } - } - } -} - -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) -{ - m_constraints.push_back(constraint); - if (disableCollisionsBetweenLinkedBodies) - { - constraint->getRigidBodyA().addConstraintRef(constraint); - constraint->getRigidBodyB().addConstraintRef(constraint); - } -} - -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) -{ - m_constraints.remove(constraint); - constraint->getRigidBodyA().removeConstraintRef(constraint); - constraint->getRigidBodyB().removeConstraintRef(constraint); -} - -void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) -{ - m_vehicles.push_back(vehicle); -} - -void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) -{ - m_vehicles.remove(vehicle); -} - -void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) -{ - m_characters.push_back(character); -} - -void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) -{ - m_characters.remove(character); -} - - -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - BT_PROFILE("solveConstraints"); - - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback - { - - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btStackAlloc* m_stackAlloc; - btDispatcher* m_dispatcher; - - InplaceSolverIslandCallback( - btContactSolverInfo& solverInfo, - btConstraintSolver* solver, - btTypedConstraint** sortedConstraints, - int numConstraints, - btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(solverInfo), - m_solver(solver), - m_sortedConstraints(sortedConstraints), - m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc), - m_dispatcher(dispatcher) - { - - } - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - if (numManifolds + m_numConstraints) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - - } - } - - }; - - //sorted version of all btTypedConstraint, based on islandId - btAlignedObjectArray sortedConstraints; - sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;iprepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - - /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); - - m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); -} - - - - -void btDiscreteDynamicsWorld::calculateSimulationIslands() -{ - BT_PROFILE("calculateSimulationIslands"); - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - if (colObj0->isActive() || colObj1->isActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } - } - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - -} - - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" - -class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback -{ - btCollisionObject* m_me; - btScalar m_allowedPenetration; - btOverlappingPairCache* m_pairCache; - - -public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : - btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_allowedPenetration(0.0f), - m_me(me), - m_pairCache(pairCache) - { - } - - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) - { - if (convexResult.m_hitCollisionObject == m_me) - return 1.0f; - - //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) - return 1.0f; - - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); - - btVector3 relativeVelocity = (linVelA-linVelB); - //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) - return 1.f; - - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); - } - - virtual bool needsCollision(btBroadphaseProxy* proxy0) const - { - //don't collide with itself - if (proxy0->m_clientObject == m_me) - return false; - - ///don't do CCD when the collision filters are not matching - if (!ClosestConvexResultCallback::needsCollision(proxy0)) - return false; - - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) - { - if (collisionPair->m_algorithm) - { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;jgetNumContacts()>0) - return false; - } - } - } - return true; - } - - -}; - -///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; - -//#include "stdio.h" -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - BT_PROFILE("integrateTransforms"); - btTransform predictedTrans; - for ( int i=0;isetHitFraction(1.f); - - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) - { - BT_PROFILE("CCD motion clamping"); - if (body->getCollisionShape()->isConvex()) - { - gNumClampedCcdMotions++; - - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); - btConvexShape* convexShape = static_cast(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); - - sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; - - convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); - if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) - { - body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); - body->setHitFraction(0.f); -// printf("clamped integration to hit fraction = %f\n",fraction); - } - } - } - - body->proceedToTransform( predictedTrans); - } - } - } -} - - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;iisStaticOrKinematicObject()) - { - - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); - - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } - } -} - - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) -{ - (void)timeStep; - -#ifndef BT_NO_PROFILE - CProfileManager::Reset(); -#endif //BT_NO_PROFILE - -} - - - - - - -class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback -{ - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; - -public: - - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } -}; - -void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) -{ - btVector3 start = transform.getOrigin(); - - const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); - const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); - const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); - - // XY - getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); - getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); - getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); - getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); - - // XZ - getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); - getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); - getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); - getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); - - // YZ - getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); - getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); - getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); - getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); -} - -void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) -{ - // Draw a small simplex at the center of the object - { - btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); - getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); - } - - if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) - { - const btCompoundShape* compoundShape = static_cast(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); - } - - } else - { - switch (shape->getShapeType()) - { - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - - debugDrawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - { - const btMultiSphereShape* multiSphereShape = static_cast(shape); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) - { - btTransform childTransform = worldTransform; - childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); - debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); - } - - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast(shape); - - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - - int upAxis = capsuleShape->getUpAxis(); - - - btVector3 capStart(0.f,0.f,0.f); - capStart[upAxis] = -halfHeight; - - btVector3 capEnd(0.f,0.f,0.f); - capEnd[upAxis] = halfHeight; - - // Draw the ends - { - - btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * capStart; - debugDrawSphere(radius, childTransform, color); - } - - { - btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * capEnd; - debugDrawSphere(radius, childTransform, color); - } - - // Draw some additional lines - btVector3 start = worldTransform.getOrigin(); - - - capStart[(upAxis+1)%3] = radius; - capEnd[(upAxis+1)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - capStart[(upAxis+1)%3] = -radius; - capEnd[(upAxis+1)%3] = -radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - - capStart[(upAxis+1)%3] = 0.f; - capEnd[(upAxis+1)%3] = 0.f; - - capStart[(upAxis+2)%3] = radius; - capEnd[(upAxis+2)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - capStart[(upAxis+2)%3] = -radius; - capEnd[(upAxis+2)%3] = -radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); - - - break; - } - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - btVector3 start = worldTransform.getOrigin(); - - int upAxis= coneShape->getConeUpIndex(); - - - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = height * btScalar(0.5); - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - btVector3 offset2Radius(0,0,0); - offset2Radius[(upAxis+2)%3] = radius; - - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); - - - - break; - - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - btVector3 start = worldTransform.getOrigin(); - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = halfHeight; - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); - break; - } - - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - btVector3 planeOrigin = planeNormal * planeConst; - btVector3 vec0,vec1; - btPlaneSpace1(planeNormal,vec0,vec1); - btScalar vecLen = 100.f; - btVector3 pt0 = planeOrigin + vec0*vecLen; - btVector3 pt1 = planeOrigin - vec0*vecLen; - btVector3 pt2 = planeOrigin + vec1*vecLen; - btVector3 pt3 = planeOrigin - vec1*vecLen; - getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); - getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); - break; - - } - default: - { - - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; - - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - for (i=0;igetNumEdges();i++) - { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - - } - - - } - } - } - } -} - - -void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) -{ - bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; - bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; - btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) - { - return; - } - - switch(constraint->getConstraintType()) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - } - break; - case HINGE_CONSTRAINT_TYPE: - { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(minAng > maxAng) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } - } - break; - case CONETWIST_CONSTRAINT_TYPE: - { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; iGetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); - - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); - - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); - - } - } - break; - case D6_CONSTRAINT_TYPE: - { - btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = p6DOF->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); - } - } - break; - case SLIDER_CONSTRAINT_TYPE: - { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->getCalculatedTransformA(); - btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); - btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); - getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis = tr.getBasis().getColumn(1); - btScalar a_min = pSlider->getLowerAngLimit(); - btScalar a_max = pSlider->getUpperAngLimit(); - const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); - } - } - break; - default : - break; - } - return; -} // btDiscreteDynamicsWorld::debugDrawConstraint() - - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree( m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} - - -int btDiscreteDynamicsWorld::getNumConstraints() const -{ - return int(m_constraints.size()); -} -btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) -{ - return m_constraints[index]; -} -const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const -{ - return m_constraints[index]; -} - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btDiscreteDynamicsWorld.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btQuickprof.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" + +//for debug rendering +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "LinearMath/btIDebugDraw.h" + + + +//vehicle +#include "BulletDynamics/Vehicle/btRaycastVehicle.h" +#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" +#include "BulletDynamics/Vehicle/btWheelInfo.h" +//character +#include "BulletDynamics/Character/btCharacterControllerInterface.h" + +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + + + + + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) +:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), +m_constraintSolver(constraintSolver), +m_gravity(0,-10,0), +m_localTime(btScalar(1.)/btScalar(60.)), +m_profileTimings(0) +{ + if (!m_constraintSolver) + { + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; + m_ownsConstraintSolver = true; + } else + { + m_ownsConstraintSolver = false; + } + + { + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + m_islandManager = new (mem) btSimulationIslandManager(); + } + + m_ownsIslandManager = true; +} + + +btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() +{ + //only delete it when we created it + if (m_ownsIslandManager) + { + m_islandManager->~btSimulationIslandManager(); + btAlignedFree( m_islandManager); + } + if (m_ownsConstraintSolver) + { + + m_constraintSolver->~btConstraintSolver(); + btAlignedFree(m_constraintSolver); + } +} + +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + + for (int i=0;igetActivationState() != ISLAND_SLEEPING) + { + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } + } + } + } +} + +void btDiscreteDynamicsWorld::debugDrawWorld() +{ + BT_PROFILE("debugDrawWorld"); + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + { + int numManifolds = getDispatcher()->getNumManifolds(); + btVector3 color(0,0,0); + for (int i=0;igetManifoldByIndexInternal(i); + //btCollisionObject* obA = static_cast(contactManifold->getBody0()); + //btCollisionObject* obB = static_cast(contactManifold->getBody1()); + + int numContacts = contactManifold->getNumContacts(); + for (int j=0;jgetContactPoint(j); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + } + } + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + } + if(drawConstraints) + { + for(int i = getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = getConstraint(i); + debugDrawConstraint(constraint); + } + } + + + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + { + int i; + + for ( i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; + case ISLAND_SLEEPING: + color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; + case WANTS_DEACTIVATION: + color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; + case DISABLE_DEACTIVATION: + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; + case DISABLE_SIMULATION: + color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; + default: + { + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); + } + }; + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + btVector3 minAabb,maxAabb; + btVector3 colorvec(1,0,0); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + } + + } + + for ( i=0;im_vehicles.size();i++) + { + for (int v=0;vgetNumWheels();v++) + { + btVector3 wheelColor(0,255,255); + if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) + { + wheelColor.setValue(0,0,255); + } else + { + wheelColor.setValue(255,0,255); + } + + btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); + + btVector3 axle = btVector3( + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); + m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); + + } + } + } +} + +void btDiscreteDynamicsWorld::clearForces() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;iclearForces(); + } + } +} + +///apply gravity, call this once per timestep +void btDiscreteDynamicsWorld::applyGravity() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;iisActive()) + { + body->applyGravity(); + } + } +} + + +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +{ + btAssert(body); + + if (body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + ///@todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } +} + + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ + BT_PROFILE("synchronizeMotionStates"); + { + //todo: iterate over awake simulation islands! + for ( int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + for ( int i=0;im_vehicles.size();i++) + { + for (int v=0;vgetNumWheels();v++) + { + //synchronize the wheels with the (interpolated) chassis worldtransform + m_vehicles[i]->updateWheelTransform(v,true); + } + } + } + +} + + +int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +{ + startProfiling(timeStep); + + BT_PROFILE("stepSimulation"); + + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int( m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = timeStep; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + btIDebugDraw* debugDrawer = getDebugDrawer (); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + + saveKinematicState(fixedTimeStep); + + applyGravity(); + + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + + for (int i=0;isetGravity(gravity); + } + } +} + +btVector3 btDiscreteDynamicsWorld::getGravity () const +{ + return m_gravity; +} + + +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + removeCollisionObject(body); +} + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +{ + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + } +} + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +{ + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + addCollisionObject(body,group,mask); + } +} + + +void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) +{ + BT_PROFILE("updateVehicles"); + + for ( int i=0;iupdateVehicle( timeStep); + } +} + +void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) +{ + BT_PROFILE("updateCharacters"); + + for ( int i=0;ipreStep (this); + character->playerStep (this,timeStep); + } +} + + + +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BT_PROFILE("updateActivationState"); + + for ( int i=0;iupdateDeactivation(timeStep); + + if (body->wantsSleeping()) + { + if (body->isStaticOrKinematicObject()) + { + body->setActivationState(ISLAND_SLEEPING); + } else + { + if (body->getActivationState() == ACTIVE_TAG) + body->setActivationState( WANTS_DEACTIVATION ); + if (body->getActivationState() == ISLAND_SLEEPING) + { + body->setAngularVelocity(btVector3(0,0,0)); + body->setLinearVelocity(btVector3(0,0,0)); + } + + } + } else + { + if (body->getActivationState() != DISABLE_DEACTIVATION) + body->setActivationState( ACTIVE_TAG ); + } + } + } +} + +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +{ + m_constraints.push_back(constraint); + if (disableCollisionsBetweenLinkedBodies) + { + constraint->getRigidBodyA().addConstraintRef(constraint); + constraint->getRigidBodyB().addConstraintRef(constraint); + } +} + +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +{ + m_constraints.remove(constraint); + constraint->getRigidBodyA().removeConstraintRef(constraint); + constraint->getRigidBodyB().removeConstraintRef(constraint); +} + +void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) +{ + m_vehicles.push_back(vehicle); +} + +void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) +{ + m_vehicles.remove(vehicle); +} + +void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) +{ + m_characters.push_back(character); +} + +void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) +{ + m_characters.remove(character); +} + + +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + + +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + BT_PROFILE("solveConstraints"); + + struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback + { + + btContactSolverInfo& m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btStackAlloc* m_stackAlloc; + btDispatcher* m_dispatcher; + + InplaceSolverIslandCallback( + btContactSolverInfo& solverInfo, + btConstraintSolver* solver, + btTypedConstraint** sortedConstraints, + int numConstraints, + btIDebugDraw* debugDrawer, + btStackAlloc* stackAlloc, + btDispatcher* dispatcher) + :m_solverInfo(solverInfo), + m_solver(solver), + m_sortedConstraints(sortedConstraints), + m_numConstraints(numConstraints), + m_debugDrawer(debugDrawer), + m_stackAlloc(stackAlloc), + m_dispatcher(dispatcher) + { + + } + + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + if (numManifolds + m_numConstraints) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + + } + } + + }; + + //sorted version of all btTypedConstraint, based on islandId + btAlignedObjectArray sortedConstraints; + sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;iprepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); +} + + + + +void btDiscreteDynamicsWorld::calculateSimulationIslands() +{ + BT_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + if (colObj0->isActive() || colObj1->isActive()) + { + + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } + } + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + + +} + + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback +{ + btCollisionObject* m_me; + btScalar m_allowedPenetration; + btOverlappingPairCache* m_pairCache; + + +public: + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btCollisionWorld::ClosestConvexResultCallback(fromA,toA), + m_allowedPenetration(0.0f), + m_me(me), + m_pairCache(pairCache) + { + } + + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return 1.0f; + + //ignore result if there is no contact response + if(!convexResult.m_hitCollisionObject->hasContactResponse()) + return 1.0f; + + btVector3 linVelA,linVelB; + linVelA = m_convexToWorld-m_convexFromWorld; + linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + + btVector3 relativeVelocity = (linVelA-linVelB); + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + return 1.f; + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //don't collide with itself + if (proxy0->m_clientObject == m_me) + return false; + + ///don't do CCD when the collision filters are not matching + if (!ClosestConvexResultCallback::needsCollision(proxy0)) + return false; + + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) + { + if (collisionPair->m_algorithm) + { + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;jgetNumContacts()>0) + return false; + } + } + } + return true; + } + + +}; + +///internal debugging variable. this value shouldn't be too high +int gNumClampedCcdMotions=0; + +//#include "stdio.h" +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + btTransform predictedTrans; + for ( int i=0;isetHitFraction(1.f); + + if (body->isActive() && (!body->isStaticOrKinematicObject())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("CCD motion clamping"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; + + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btConvexShape* convexShape = static_cast(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(body->getCollisionShape()); + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + body->setHitFraction(sweepResults.m_closestHitFraction); + body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->setHitFraction(0.f); +// printf("clamped integration to hit fraction = %f\n",fraction); + } + } + } + + body->proceedToTransform( predictedTrans); + } + } + } +} + + + + + +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + for ( int i=0;iisStaticOrKinematicObject()) + { + + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); + + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + } + } + } +} + + +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) +{ + (void)timeStep; + +#ifndef BT_NO_PROFILE + CProfileManager::Reset(); +#endif //BT_NO_PROFILE + +} + + + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; + +public: + + DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : + m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + processTriangle(triangle,partId,triangleIndex); + } + + virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + btVector3 wv0,wv1,wv2; + wv0 = m_worldTrans*triangle[0]; + wv1 = m_worldTrans*triangle[1]; + wv2 = m_worldTrans*triangle[2]; + m_debugDrawer->drawLine(wv0,wv1,m_color); + m_debugDrawer->drawLine(wv1,wv2,m_color); + m_debugDrawer->drawLine(wv2,wv0,m_color); + } +}; + +void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) +{ + btVector3 start = transform.getOrigin(); + + const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); + const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); + const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); + + // XY + getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); + getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); + + // XZ + getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); + + // YZ + getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); +} + +void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) +{ + // Draw a small simplex at the center of the object + { + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); + } + + if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + const btCompoundShape* compoundShape = static_cast(shape); + for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* colShape = compoundShape->getChildShape(i); + debugDrawObject(worldTransform*childTrans,colShape,color); + } + + } else + { + switch (shape->getShapeType()) + { + + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast(shape); + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + + debugDrawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast(shape); + + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); + debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + int upAxis = capsuleShape->getUpAxis(); + + + btVector3 capStart(0.f,0.f,0.f); + capStart[upAxis] = -halfHeight; + + btVector3 capEnd(0.f,0.f,0.f); + capEnd[upAxis] = halfHeight; + + // Draw the ends + { + + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * capStart; + debugDrawSphere(radius, childTransform, color); + } + + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * capEnd; + debugDrawSphere(radius, childTransform, color); + } + + // Draw some additional lines + btVector3 start = worldTransform.getOrigin(); + + + capStart[(upAxis+1)%3] = radius; + capEnd[(upAxis+1)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + capStart[(upAxis+1)%3] = -radius; + capEnd[(upAxis+1)%3] = -radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + + capStart[(upAxis+1)%3] = 0.f; + capEnd[(upAxis+1)%3] = 0.f; + + capStart[(upAxis+2)%3] = radius; + capEnd[(upAxis+2)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + capStart[(upAxis+2)%3] = -radius; + capEnd[(upAxis+2)%3] = -radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + + + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast(shape); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); + btVector3 start = worldTransform.getOrigin(); + + int upAxis= coneShape->getConeUpIndex(); + + + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = height * btScalar(0.5); + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + btVector3 offset2Radius(0,0,0); + offset2Radius[(upAxis+2)%3] = radius; + + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); + + + + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + btVector3 start = worldTransform.getOrigin(); + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = halfHeight; + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + break; + } + + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0,vec1; + btPlaneSpace1(planeNormal,vec0,vec1); + btScalar vecLen = 100.f; + btVector3 pt0 = planeOrigin + vec0*vecLen; + btVector3 pt1 = planeOrigin - vec0*vecLen; + btVector3 pt2 = planeOrigin + vec1*vecLen; + btVector3 pt3 = planeOrigin - vec1*vecLen; + getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); + getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); + break; + + } + default: + { + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + for (i=0;igetNumEdges();i++) + { + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + + } + + + } + } + } + } +} + + +void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) +{ + bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; + bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; + btScalar dbgDrawSize = constraint->getDbgDrawSize(); + if(dbgDrawSize <= btScalar(0.f)) + { + return; + } + + switch(constraint->getConstraintType()) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if(minAng == maxAng) + { + break; + } + bool drawSect = true; + if(minAng > maxAng) + { + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; + } + if(drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); + } + } + break; + case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8*4; + btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i=0; iGetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + + if (i%(nSegments/8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); + + pPrev = pCur; + } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if(useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + + } + } + break; + case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; + ref[1] = -sz*axis[0] + cz*axis[1]; + ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + tr = p6DOF->getCalculatedTransformB(); + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if(minFi > maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); + } + else if(minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); + } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + } + } + break; + case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if(drawLimits) + { + btTransform tr = pSlider->getCalculatedTransformA(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); + } + } + break; + default : + break; + } + return; +} // btDiscreteDynamicsWorld::debugDrawConstraint() + + + + + +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + btAlignedFree( m_constraintSolver); + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; +} + +btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} + + +int btDiscreteDynamicsWorld::getNumConstraints() const +{ + return int(m_constraints.size()); +} +btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) +{ + return m_constraints[index]; +} +const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const +{ + return m_constraints[index]; +} + + diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index c64969f22..34d4e6b35 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -1,176 +1,176 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DISCRETE_DYNAMICS_WORLD_H -#define BT_DISCRETE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; - - -class btRaycastVehicle; -class btCharacterControllerInterface; -class btIDebugDraw; -#include "LinearMath/btAlignedObjectArray.h" - - -///btDiscreteDynamicsWorld provides discrete rigid body simulation -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -class btDiscreteDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray m_constraints; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - - - btAlignedObjectArray m_vehicles; - - btAlignedObjectArray m_characters; - - - int m_profileTimings; - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - virtual void calculateSimulationIslands(); - - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - void updateActivationState(btScalar timeStep); - - void updateVehicles(btScalar timeStep); - - void updateCharacters(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - - virtual void saveKinematicState(btScalar timeStep); - - void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); - - -public: - - - ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btDiscreteDynamicsWorld(); - - ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - - virtual void synchronizeMotionStates(); - - ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - - virtual void removeConstraint(btTypedConstraint* constraint); - - virtual void addVehicle(btRaycastVehicle* vehicle); - - virtual void removeVehicle(btRaycastVehicle* vehicle); - - virtual void addCharacter(btCharacterControllerInterface* character); - - virtual void removeCharacter(btCharacterControllerInterface* character); - - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; - - virtual void addRigidBody(btRigidBody* body); - - virtual void addRigidBody(btRigidBody* body, short group, short mask); - - virtual void removeRigidBody(btRigidBody* body); - - void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - - void debugDrawConstraint(btTypedConstraint* constraint); - - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) const; - - - virtual btDynamicsWorldType getWorldType() const - { - return BT_DISCRETE_DYNAMICS_WORLD; - } - - ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. - virtual void clearForces(); - - ///apply gravity, call this once per timestep - virtual void applyGravity(); - - virtual void setNumTasks(int numTasks) - { - (void) numTasks; - } - -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_H +#define BT_DISCRETE_DYNAMICS_WORLD_H + +#include "btDynamicsWorld.h" + +class btDispatcher; +class btOverlappingPairCache; +class btConstraintSolver; +class btSimulationIslandManager; +class btTypedConstraint; + + +class btRaycastVehicle; +class btCharacterControllerInterface; +class btIDebugDraw; +#include "LinearMath/btAlignedObjectArray.h" + + +///btDiscreteDynamicsWorld provides discrete rigid body simulation +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btDiscreteDynamicsWorld : public btDynamicsWorld +{ +protected: + + btConstraintSolver* m_constraintSolver; + + btSimulationIslandManager* m_islandManager; + + btAlignedObjectArray m_constraints; + + btVector3 m_gravity; + + //for variable timesteps + btScalar m_localTime; + //for variable timesteps + + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + + + btAlignedObjectArray m_vehicles; + + btAlignedObjectArray m_characters; + + + int m_profileTimings; + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void integrateTransforms(btScalar timeStep); + + virtual void calculateSimulationIslands(); + + virtual void solveConstraints(btContactSolverInfo& solverInfo); + + void updateActivationState(btScalar timeStep); + + void updateVehicles(btScalar timeStep); + + void updateCharacters(btScalar timeStep); + + void startProfiling(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + + virtual void saveKinematicState(btScalar timeStep); + + void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + + +public: + + + ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those + btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btDiscreteDynamicsWorld(); + + ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + + + virtual void synchronizeMotionStates(); + + ///this can be useful to synchronize a single rigid body -> graphics object + void synchronizeSingleMotionState(btRigidBody* body); + + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + + virtual void removeConstraint(btTypedConstraint* constraint); + + virtual void addVehicle(btRaycastVehicle* vehicle); + + virtual void removeVehicle(btRaycastVehicle* vehicle); + + virtual void addCharacter(btCharacterControllerInterface* character); + + virtual void removeCharacter(btCharacterControllerInterface* character); + + + btSimulationIslandManager* getSimulationIslandManager() + { + return m_islandManager; + } + + const btSimulationIslandManager* getSimulationIslandManager() const + { + return m_islandManager; + } + + btCollisionWorld* getCollisionWorld() + { + return this; + } + + virtual void setGravity(const btVector3& gravity); + virtual btVector3 getGravity () const; + + virtual void addRigidBody(btRigidBody* body); + + virtual void addRigidBody(btRigidBody* body, short group, short mask); + + virtual void removeRigidBody(btRigidBody* body); + + void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + + void debugDrawConstraint(btTypedConstraint* constraint); + + virtual void debugDrawWorld(); + + virtual void setConstraintSolver(btConstraintSolver* solver); + + virtual btConstraintSolver* getConstraintSolver(); + + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index) ; + + virtual const btTypedConstraint* getConstraint(int index) const; + + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DISCRETE_DYNAMICS_WORLD; + } + + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. + virtual void clearForces(); + + ///apply gravity, call this once per timestep + virtual void applyGravity(); + + virtual void setNumTasks(int numTasks) + { + (void) numTasks; + } + +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 2346deba1..2d90e212f 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -1,136 +1,136 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DYNAMICS_WORLD_H -#define BT_DYNAMICS_WORLD_H - -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - -class btTypedConstraint; -class btRaycastVehicle; -class btConstraintSolver; -class btDynamicsWorld; -class btCharacterControllerInterface; - -/// Type for the callback for each tick -typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); - -enum btDynamicsWorldType -{ - BT_SIMPLE_DYNAMICS_WORLD=1, - BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3 -}; - -///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. -class btDynamicsWorld : public btCollisionWorld -{ - -protected: - btInternalTickCallback m_internalTickCallback; - void* m_worldUserInfo; - - btContactSolverInfo m_solverInfo; - -public: - - - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0) - { - } - - virtual ~btDynamicsWorld() - { - } - - ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. - ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. - ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. - ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void debugDrawWorld() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) - { - (void)constraint; (void)disableCollisionsBetweenLinkedBodies; - } - - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} - - virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} - - virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} - - virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} - - virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;} - - - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - virtual btVector3 getGravity () const = 0; - - virtual void synchronizeMotionStates() = 0; - - virtual void addRigidBody(btRigidBody* body) = 0; - - virtual void removeRigidBody(btRigidBody* body) = 0; - - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } - - virtual btDynamicsWorldType getWorldType() const=0; - - virtual void clearForces() = 0; - - /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0) - { - m_internalTickCallback = cb; - m_worldUserInfo = worldUserInfo; - } - - void setWorldUserInfo(void* worldUserInfo) - { - m_worldUserInfo = worldUserInfo; - } - - void* getWorldUserInfo() const - { - return m_worldUserInfo; - } - - btContactSolverInfo& getSolverInfo() - { - return m_solverInfo; - } - - -}; - -#endif //BT_DYNAMICS_WORLD_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DYNAMICS_WORLD_H +#define BT_DYNAMICS_WORLD_H + +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + +class btTypedConstraint; +class btRaycastVehicle; +class btConstraintSolver; +class btDynamicsWorld; +class btCharacterControllerInterface; + +/// Type for the callback for each tick +typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); + +enum btDynamicsWorldType +{ + BT_SIMPLE_DYNAMICS_WORLD=1, + BT_DISCRETE_DYNAMICS_WORLD=2, + BT_CONTINUOUS_DYNAMICS_WORLD=3 +}; + +///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. +class btDynamicsWorld : public btCollisionWorld +{ + +protected: + btInternalTickCallback m_internalTickCallback; + void* m_worldUserInfo; + + btContactSolverInfo m_solverInfo; + +public: + + + btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) + :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0) + { + } + + virtual ~btDynamicsWorld() + { + } + + ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. + ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. + ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. + ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; + + virtual void debugDrawWorld() = 0; + + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) + { + (void)constraint; (void)disableCollisionsBetweenLinkedBodies; + } + + virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} + + virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} + + virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} + + virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} + + virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;} + + + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + virtual void setGravity(const btVector3& gravity) = 0; + virtual btVector3 getGravity () const = 0; + + virtual void synchronizeMotionStates() = 0; + + virtual void addRigidBody(btRigidBody* body) = 0; + + virtual void removeRigidBody(btRigidBody* body) = 0; + + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + + virtual btConstraintSolver* getConstraintSolver() = 0; + + virtual int getNumConstraints() const { return 0; } + + virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } + + virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } + + virtual btDynamicsWorldType getWorldType() const=0; + + virtual void clearForces() = 0; + + /// Set the callback for when an internal tick (simulation substep) happens, optional user info + void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0) + { + m_internalTickCallback = cb; + m_worldUserInfo = worldUserInfo; + } + + void setWorldUserInfo(void* worldUserInfo) + { + m_worldUserInfo = worldUserInfo; + } + + void* getWorldUserInfo() const + { + return m_worldUserInfo; + } + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + +}; + +#endif //BT_DYNAMICS_WORLD_H + + diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 04872d130..3f6141463 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -1,243 +1,243 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSimpleDynamicsWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - - -/* - Make sure this dummy function never changes so that it - can be used by probes that are checking whether the - library is actually installed. -*/ -extern "C" -{ - void btBulletDynamicsProbe (); - void btBulletDynamicsProbe () {} -} - - - - -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_ownsConstraintSolver(false), -m_gravity(0,0,-10) -{ - -} - - -btSimpleDynamicsWorld::~btSimpleDynamicsWorld() -{ - if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); -} - -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - (void)fixedTimeStep; - (void)maxSubSteps; - - - ///apply gravity, predict motion - predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = getDebugDrawer(); - - ///perform collision detection - performDiscreteCollisionDetection(); - - ///solve contact constraints - int numManifolds = m_dispatcher1->getNumManifolds(); - if (numManifolds) - { - btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); - - btContactSolverInfo infoGlobal; - infoGlobal.m_timeStep = timeStep; - m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); - m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); - } - - ///integrate transforms - integrateTransforms(timeStep); - - updateAabbs(); - - synchronizeMotionStates(); - - clearForces(); - - return 1; - -} - -void btSimpleDynamicsWorld::clearForces() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;iclearForces(); - } - } -} - - -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; - for ( int i=0;isetGravity(gravity); - } - } -} - -btVector3 btSimpleDynamicsWorld::getGravity () const -{ - return m_gravity; -} - -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - removeCollisionObject(body); -} - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) -{ - body->setGravity(m_gravity); - - if (body->getCollisionShape()) - { - addCollisionObject(body); - } -} - -void btSimpleDynamicsWorld::updateAabbs() -{ - btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) - { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); - } - } - } -} - -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - } - } - } -} - - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - for ( int i=0;iisStaticObject()) - { - if (body->isActive()) - { - body->applyGravity(); - body->integrateVelocities( timeStep); - body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } - } - } - } -} - - -void btSimpleDynamicsWorld::synchronizeMotionStates() -{ - ///@todo: iterate over awake simulation islands! - for ( int i=0;igetMotionState()) - { - if (body->getActivationState() != ISLAND_SLEEPING) - { - body->getMotionState()->setWorldTransform(body->getWorldTransform()); - } - } - } - -} - - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - btAlignedFree(m_constraintSolver); - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSimpleDynamicsWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" +{ + void btBulletDynamicsProbe (); + void btBulletDynamicsProbe () {} +} + + + + +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), +m_constraintSolver(constraintSolver), +m_ownsConstraintSolver(false), +m_gravity(0,0,-10) +{ + +} + + +btSimpleDynamicsWorld::~btSimpleDynamicsWorld() +{ + if (m_ownsConstraintSolver) + btAlignedFree( m_constraintSolver); +} + +int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +{ + (void)fixedTimeStep; + (void)maxSubSteps; + + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + ///perform collision detection + performDiscreteCollisionDetection(); + + ///solve contact constraints + int numManifolds = m_dispatcher1->getNumManifolds(); + if (numManifolds) + { + btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); + + btContactSolverInfo infoGlobal; + infoGlobal.m_timeStep = timeStep; + m_constraintSolver->prepareSolve(0,numManifolds); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); + } + + ///integrate transforms + integrateTransforms(timeStep); + + updateAabbs(); + + synchronizeMotionStates(); + + clearForces(); + + return 1; + +} + +void btSimpleDynamicsWorld::clearForces() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;iclearForces(); + } + } +} + + +void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; + for ( int i=0;isetGravity(gravity); + } + } +} + +btVector3 btSimpleDynamicsWorld::getGravity () const +{ + return m_gravity; +} + +void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + removeCollisionObject(body); +} + +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) +{ + body->setGravity(m_gravity); + + if (body->getCollisionShape()) + { + addCollisionObject(body); + } +} + +void btSimpleDynamicsWorld::updateAabbs() +{ + btTransform predictedTrans; + for ( int i=0;iisActive() && (!body->isStaticObject())) + { + btVector3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btBroadphaseInterface* bp = getBroadphase(); + bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + } + } + } +} + +void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + btTransform predictedTrans; + for ( int i=0;iisActive() && (!body->isStaticObject())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + body->proceedToTransform( predictedTrans); + } + } + } +} + + + +void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + for ( int i=0;iisStaticObject()) + { + if (body->isActive()) + { + body->applyGravity(); + body->integrateVelocities( timeStep); + body->applyDamping(timeStep); + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + } + } + } + } +} + + +void btSimpleDynamicsWorld::synchronizeMotionStates() +{ + ///@todo: iterate over awake simulation islands! + for ( int i=0;igetMotionState()) + { + if (body->getActivationState() != ISLAND_SLEEPING) + { + body->getMotionState()->setWorldTransform(body->getWorldTransform()); + } + } + } + +} + + +void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + btAlignedFree(m_constraintSolver); + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; +} + +btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index f5223e558..4982e6730 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -1,729 +1,729 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ - -#include "LinearMath/btVector3.h" -#include "btRaycastVehicle.h" - -#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" -#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" -#include "LinearMath/btQuaternion.h" -#include "BulletDynamics/Dynamics/btDynamicsWorld.h" -#include "btVehicleRaycaster.h" -#include "btWheelInfo.h" -#include "LinearMath/btMinMax.h" - - -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - -static btRigidBody s_fixedObject( 0,0,0); - -btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) -: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE), -m_vehicleRaycaster(raycaster), -m_pitchControl(btScalar(0.)) -{ - m_chassisBody = chassis; - m_indexRightAxis = 0; - m_indexUpAxis = 2; - m_indexForwardAxis = 1; - defaultInit(tuning); -} - - -void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) -{ - (void)tuning; - m_currentVehicleSpeedKmHour = btScalar(0.); - m_steeringValue = btScalar(0.); - -} - - - -btRaycastVehicle::~btRaycastVehicle() -{ -} - - -// -// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed -// -btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) -{ - - btWheelInfoConstructionInfo ci; - - ci.m_chassisConnectionCS = connectionPointCS; - ci.m_wheelDirectionCS = wheelDirectionCS0; - ci.m_wheelAxleCS = wheelAxleCS; - ci.m_suspensionRestLength = suspensionRestLength; - ci.m_wheelRadius = wheelRadius; - ci.m_suspensionStiffness = tuning.m_suspensionStiffness; - ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; - ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; - ci.m_frictionSlip = tuning.m_frictionSlip; - ci.m_bIsFrontWheel = isFrontWheel; - ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; - - m_wheelInfo.push_back( btWheelInfo(ci)); - - btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; - - updateWheelTransformsWS( wheel , false ); - updateWheelTransform(getNumWheels()-1,false); - return wheel; -} - - - - -const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const -{ - btAssert(wheelIndex < getNumWheels()); - const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; - return wheel.m_worldTransform; - -} - -void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) -{ - - btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; - updateWheelTransformsWS(wheel,interpolatedTransform); - btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; - const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; - btVector3 fwd = up.cross(right); - fwd = fwd.normalize(); -// up = right.cross(fwd); -// up.normalize(); - - //rotate around steering over de wheelAxleWS - btScalar steering = wheel.m_steering; - - btQuaternion steeringOrn(up,steering);//wheel.m_steering); - btMatrix3x3 steeringMat(steeringOrn); - - btQuaternion rotatingOrn(right,-wheel.m_rotation); - btMatrix3x3 rotatingMat(rotatingOrn); - - btMatrix3x3 basis2( - right[0],fwd[0],up[0], - right[1],fwd[1],up[1], - right[2],fwd[2],up[2] - ); - - wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); - wheel.m_worldTransform.setOrigin( - wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength - ); -} - -void btRaycastVehicle::resetSuspension() -{ - - int i; - for (i=0;igetMotionState())) - { - getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); - } - - wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); - wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; - wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; -} - -btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) -{ - updateWheelTransformsWS( wheel,false); - - - btScalar depth = -1; - - btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; - - btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); - const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; - wheel.m_raycastInfo.m_contactPointWS = source + rayvector; - const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; - - btScalar param = btScalar(0.); - - btVehicleRaycaster::btVehicleRaycasterResult rayResults; - - btAssert(m_vehicleRaycaster); - - void* object = m_vehicleRaycaster->castRay(source,target,rayResults); - - wheel.m_raycastInfo.m_groundObject = 0; - - if (object) - { - param = rayResults.m_distFraction; - depth = raylen * rayResults.m_distFraction; - wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; - wheel.m_raycastInfo.m_isInContact = true; - - wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!; - //wheel.m_raycastInfo.m_groundObject = object; - - - btScalar hitDistance = param*raylen; - wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; - //clamp on max suspension travel - - btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); - btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); - if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; - } - if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) - { - wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; - } - - wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; - - btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); - - btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); - - chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); - - btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - - if ( denominator >= btScalar(-0.1)) - { - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / denominator; - wheel.m_suspensionRelativeVelocity = projVel * inv; - wheel.m_clippedInvContactDotSuspension = inv; - } - - } else - { - //put wheel info as in rest position - wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); - wheel.m_suspensionRelativeVelocity = btScalar(0.0); - wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; - wheel.m_clippedInvContactDotSuspension = btScalar(1.0); - } - - return depth; -} - - -const btTransform& btRaycastVehicle::getChassisWorldTransform() const -{ - /*if (getRigidBody()->getMotionState()) - { - btTransform chassisWorldTrans; - getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); - return chassisWorldTrans; - } - */ - - - return getRigidBody()->getCenterOfMassTransform(); -} - - -void btRaycastVehicle::updateVehicle( btScalar step ) -{ - { - for (int i=0;igetLinearVelocity().length(); - - const btTransform& chassisTrans = getChassisWorldTransform(); - - btVector3 forwardW ( - chassisTrans.getBasis()[0][m_indexForwardAxis], - chassisTrans.getBasis()[1][m_indexForwardAxis], - chassisTrans.getBasis()[2][m_indexForwardAxis]); - - if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) - { - m_currentVehicleSpeedKmHour *= btScalar(-1.); - } - - // - // simulate suspension - // - - int i=0; - for (i=0;i gMaxSuspensionForce) - { - suspensionForce = gMaxSuspensionForce; - } - btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; - btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); - - getRigidBody()->applyImpulse(impulse, relpos); - - } - - - - updateFriction( step); - - - for (i=0;igetCenterOfMassPosition(); - btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); - - if (wheel.m_raycastInfo.m_isInContact) - { - const btTransform& chassisWorldTransform = getChassisWorldTransform(); - - btVector3 fwd ( - chassisWorldTransform.getBasis()[0][m_indexForwardAxis], - chassisWorldTransform.getBasis()[1][m_indexForwardAxis], - chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); - - btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); - fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; - - btScalar proj2 = fwd.dot(vel); - - wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); - wheel.m_rotation += wheel.m_deltaRotation; - - } else - { - wheel.m_rotation += wheel.m_deltaRotation; - } - - wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact - - } - - - -} - - -void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_steering = steering; -} - - - -btScalar btRaycastVehicle::getSteeringValue(int wheel) const -{ - return getWheelInfo(wheel).m_steering; -} - - -void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) -{ - btAssert(wheel>=0 && wheel < getNumWheels()); - btWheelInfo& wheelInfo = getWheelInfo(wheel); - wheelInfo.m_engineForce = force; -} - - -const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -btWheelInfo& btRaycastVehicle::getWheelInfo(int index) -{ - btAssert((index >= 0) && (index < getNumWheels())); - - return m_wheelInfo[index]; -} - -void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) -{ - btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); - getWheelInfo(wheelIndex).m_brake = brake; -} - - -void btRaycastVehicle::updateSuspension(btScalar deltaTime) -{ - (void)deltaTime; - - btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); - - for (int w_it=0; w_itcomputeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); - btScalar relaxation = 1.f; - m_jacDiagABInv = relaxation/(denom0+denom1); - } - - - -}; - -btScalar calcRollingFriction(btWheelContactPoint& contactPoint); -btScalar calcRollingFriction(btWheelContactPoint& contactPoint) -{ - - btScalar j1=0.f; - - const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; - - btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); - btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); - - btScalar maxImpulse = contactPoint.m_maxImpulse; - - btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; - - btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); - - // calculate j that moves us to zero relative velocity - j1 = -vrel * contactPoint.m_jacDiagABInv; - btSetMin(j1, maxImpulse); - btSetMax(j1, -maxImpulse); - - return j1; -} - - - - -btScalar sideFrictionStiffness2 = btScalar(1.0); -void btRaycastVehicle::updateFriction(btScalar timeStep) -{ - - //calculate the impulse, so that the wheels don't move sidewards - int numWheel = getNumWheels(); - if (!numWheel) - return; - - m_forwardWS.resize(numWheel); - m_axle.resize(numWheel); - m_forwardImpulse.resize(numWheel); - m_sideImpulse.resize(numWheel); - - int numWheelsOnGround = 0; - - - //collapse all those loops into one! - for (int i=0;i maximpSquared) - { - sliding = true; - - btScalar factor = maximp / btSqrt(impulseSquared); - - m_wheelInfo[wheel].m_skidInfo *= factor; - } - } - - } - } - - - - - if (sliding) - { - for (int wheel = 0;wheel < getNumWheels(); wheel++) - { - if (m_sideImpulse[wheel] != btScalar(0.)) - { - if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) - { - m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; - } - } - } - } - - // apply the impulses - { - for (int wheel = 0;wheelgetCenterOfMassPosition(); - - if (m_forwardImpulse[wheel] != btScalar(0.)) - { - m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); - } - if (m_sideImpulse[wheel] != btScalar(0.)) - { - class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; - - btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - - groundObject->getCenterOfMassPosition(); - - - btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; - - rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence; - m_chassisBody->applyImpulse(sideImp,rel_pos); - - //apply friction impulse on the ground - groundObject->applyImpulse(-sideImp,rel_pos2); - } - } - } - - -} - - -void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) -{ -// RayResultCallback& resultCallback; - - btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); - - m_dynamicsWorld->rayTest(from, to, rayCallback); - - if (rayCallback.hasHit()) - { - - btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); - if (body && body->hasContactResponse()) - { - result.m_hitPointInWorld = rayCallback.m_hitPointWorld; - result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; - result.m_hitNormalInWorld.normalize(); - result.m_distFraction = rayCallback.m_closestHitFraction; - return body; - } - } - return 0; -} +/* + * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * Permission to use, copy, modify, distribute and sell this software + * and its documentation for any purpose is hereby granted without fee, + * provided that the above copyright notice appear in all copies. + * Erwin Coumans makes no representations about the suitability + * of this software for any purpose. + * It is provided "as is" without express or implied warranty. +*/ + +#include "LinearMath/btVector3.h" +#include "btRaycastVehicle.h" + +#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "LinearMath/btQuaternion.h" +#include "BulletDynamics/Dynamics/btDynamicsWorld.h" +#include "btVehicleRaycaster.h" +#include "btWheelInfo.h" +#include "LinearMath/btMinMax.h" + + +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + +static btRigidBody s_fixedObject( 0,0,0); + +btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) +: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE), +m_vehicleRaycaster(raycaster), +m_pitchControl(btScalar(0.)) +{ + m_chassisBody = chassis; + m_indexRightAxis = 0; + m_indexUpAxis = 2; + m_indexForwardAxis = 1; + defaultInit(tuning); +} + + +void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) +{ + (void)tuning; + m_currentVehicleSpeedKmHour = btScalar(0.); + m_steeringValue = btScalar(0.); + +} + + + +btRaycastVehicle::~btRaycastVehicle() +{ +} + + +// +// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed +// +btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) +{ + + btWheelInfoConstructionInfo ci; + + ci.m_chassisConnectionCS = connectionPointCS; + ci.m_wheelDirectionCS = wheelDirectionCS0; + ci.m_wheelAxleCS = wheelAxleCS; + ci.m_suspensionRestLength = suspensionRestLength; + ci.m_wheelRadius = wheelRadius; + ci.m_suspensionStiffness = tuning.m_suspensionStiffness; + ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; + ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; + ci.m_frictionSlip = tuning.m_frictionSlip; + ci.m_bIsFrontWheel = isFrontWheel; + ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; + + m_wheelInfo.push_back( btWheelInfo(ci)); + + btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; + + updateWheelTransformsWS( wheel , false ); + updateWheelTransform(getNumWheels()-1,false); + return wheel; +} + + + + +const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const +{ + btAssert(wheelIndex < getNumWheels()); + const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; + return wheel.m_worldTransform; + +} + +void btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedTransform) +{ + + btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; + updateWheelTransformsWS(wheel,interpolatedTransform); + btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; + const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; + btVector3 fwd = up.cross(right); + fwd = fwd.normalize(); +// up = right.cross(fwd); +// up.normalize(); + + //rotate around steering over de wheelAxleWS + btScalar steering = wheel.m_steering; + + btQuaternion steeringOrn(up,steering);//wheel.m_steering); + btMatrix3x3 steeringMat(steeringOrn); + + btQuaternion rotatingOrn(right,-wheel.m_rotation); + btMatrix3x3 rotatingMat(rotatingOrn); + + btMatrix3x3 basis2( + right[0],fwd[0],up[0], + right[1],fwd[1],up[1], + right[2],fwd[2],up[2] + ); + + wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); + wheel.m_worldTransform.setOrigin( + wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength + ); +} + +void btRaycastVehicle::resetSuspension() +{ + + int i; + for (i=0;igetMotionState())) + { + getRigidBody()->getMotionState()->getWorldTransform(chassisTrans); + } + + wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); + wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; + wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; +} + +btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) +{ + updateWheelTransformsWS( wheel,false); + + + btScalar depth = -1; + + btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; + + btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); + const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; + wheel.m_raycastInfo.m_contactPointWS = source + rayvector; + const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; + + btScalar param = btScalar(0.); + + btVehicleRaycaster::btVehicleRaycasterResult rayResults; + + btAssert(m_vehicleRaycaster); + + void* object = m_vehicleRaycaster->castRay(source,target,rayResults); + + wheel.m_raycastInfo.m_groundObject = 0; + + if (object) + { + param = rayResults.m_distFraction; + depth = raylen * rayResults.m_distFraction; + wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; + wheel.m_raycastInfo.m_isInContact = true; + + wheel.m_raycastInfo.m_groundObject = &s_fixedObject;///@todo for driving on dynamic/movable objects!; + //wheel.m_raycastInfo.m_groundObject = object; + + + btScalar hitDistance = param*raylen; + wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; + //clamp on max suspension travel + + btScalar minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*btScalar(0.01); + btScalar maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*btScalar(0.01); + if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; + } + if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; + } + + wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; + + btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); + + btVector3 chassis_velocity_at_contactPoint; + btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); + + chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); + + btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + + if ( denominator >= btScalar(-0.1)) + { + wheel.m_suspensionRelativeVelocity = btScalar(0.0); + wheel.m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); + } + else + { + btScalar inv = btScalar(-1.) / denominator; + wheel.m_suspensionRelativeVelocity = projVel * inv; + wheel.m_clippedInvContactDotSuspension = inv; + } + + } else + { + //put wheel info as in rest position + wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); + wheel.m_suspensionRelativeVelocity = btScalar(0.0); + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + wheel.m_clippedInvContactDotSuspension = btScalar(1.0); + } + + return depth; +} + + +const btTransform& btRaycastVehicle::getChassisWorldTransform() const +{ + /*if (getRigidBody()->getMotionState()) + { + btTransform chassisWorldTrans; + getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); + return chassisWorldTrans; + } + */ + + + return getRigidBody()->getCenterOfMassTransform(); +} + + +void btRaycastVehicle::updateVehicle( btScalar step ) +{ + { + for (int i=0;igetLinearVelocity().length(); + + const btTransform& chassisTrans = getChassisWorldTransform(); + + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.)) + { + m_currentVehicleSpeedKmHour *= btScalar(-1.); + } + + // + // simulate suspension + // + + int i=0; + for (i=0;i gMaxSuspensionForce) + { + suspensionForce = gMaxSuspensionForce; + } + btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; + btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); + + getRigidBody()->applyImpulse(impulse, relpos); + + } + + + + updateFriction( step); + + + for (i=0;igetCenterOfMassPosition(); + btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); + + if (wheel.m_raycastInfo.m_isInContact) + { + const btTransform& chassisWorldTransform = getChassisWorldTransform(); + + btVector3 fwd ( + chassisWorldTransform.getBasis()[0][m_indexForwardAxis], + chassisWorldTransform.getBasis()[1][m_indexForwardAxis], + chassisWorldTransform.getBasis()[2][m_indexForwardAxis]); + + btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); + fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; + + btScalar proj2 = fwd.dot(vel); + + wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); + wheel.m_rotation += wheel.m_deltaRotation; + + } else + { + wheel.m_rotation += wheel.m_deltaRotation; + } + + wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact + + } + + + +} + + +void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) +{ + btAssert(wheel>=0 && wheel < getNumWheels()); + + btWheelInfo& wheelInfo = getWheelInfo(wheel); + wheelInfo.m_steering = steering; +} + + + +btScalar btRaycastVehicle::getSteeringValue(int wheel) const +{ + return getWheelInfo(wheel).m_steering; +} + + +void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) +{ + btAssert(wheel>=0 && wheel < getNumWheels()); + btWheelInfo& wheelInfo = getWheelInfo(wheel); + wheelInfo.m_engineForce = force; +} + + +const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const +{ + btAssert((index >= 0) && (index < getNumWheels())); + + return m_wheelInfo[index]; +} + +btWheelInfo& btRaycastVehicle::getWheelInfo(int index) +{ + btAssert((index >= 0) && (index < getNumWheels())); + + return m_wheelInfo[index]; +} + +void btRaycastVehicle::setBrake(btScalar brake,int wheelIndex) +{ + btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels())); + getWheelInfo(wheelIndex).m_brake = brake; +} + + +void btRaycastVehicle::updateSuspension(btScalar deltaTime) +{ + (void)deltaTime; + + btScalar chassisMass = btScalar(1.) / m_chassisBody->getInvMass(); + + for (int w_it=0; w_itcomputeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar denom1 = body1->computeImpulseDenominator(frictionPosWorld,frictionDirectionWorld); + btScalar relaxation = 1.f; + m_jacDiagABInv = relaxation/(denom0+denom1); + } + + + +}; + +btScalar calcRollingFriction(btWheelContactPoint& contactPoint); +btScalar calcRollingFriction(btWheelContactPoint& contactPoint) +{ + + btScalar j1=0.f; + + const btVector3& contactPosWorld = contactPoint.m_frictionPositionWorld; + + btVector3 rel_pos1 = contactPosWorld - contactPoint.m_body0->getCenterOfMassPosition(); + btVector3 rel_pos2 = contactPosWorld - contactPoint.m_body1->getCenterOfMassPosition(); + + btScalar maxImpulse = contactPoint.m_maxImpulse; + + btVector3 vel1 = contactPoint.m_body0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = contactPoint.m_body1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar vrel = contactPoint.m_frictionDirectionWorld.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * contactPoint.m_jacDiagABInv; + btSetMin(j1, maxImpulse); + btSetMax(j1, -maxImpulse); + + return j1; +} + + + + +btScalar sideFrictionStiffness2 = btScalar(1.0); +void btRaycastVehicle::updateFriction(btScalar timeStep) +{ + + //calculate the impulse, so that the wheels don't move sidewards + int numWheel = getNumWheels(); + if (!numWheel) + return; + + m_forwardWS.resize(numWheel); + m_axle.resize(numWheel); + m_forwardImpulse.resize(numWheel); + m_sideImpulse.resize(numWheel); + + int numWheelsOnGround = 0; + + + //collapse all those loops into one! + for (int i=0;i maximpSquared) + { + sliding = true; + + btScalar factor = maximp / btSqrt(impulseSquared); + + m_wheelInfo[wheel].m_skidInfo *= factor; + } + } + + } + } + + + + + if (sliding) + { + for (int wheel = 0;wheel < getNumWheels(); wheel++) + { + if (m_sideImpulse[wheel] != btScalar(0.)) + { + if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.)) + { + m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + } + } + } + } + + // apply the impulses + { + for (int wheel = 0;wheelgetCenterOfMassPosition(); + + if (m_forwardImpulse[wheel] != btScalar(0.)) + { + m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos); + } + if (m_sideImpulse[wheel] != btScalar(0.)) + { + class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; + + btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - + groundObject->getCenterOfMassPosition(); + + + btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; + + rel_pos[m_indexForwardAxis] *= wheelInfo.m_rollInfluence; + m_chassisBody->applyImpulse(sideImp,rel_pos); + + //apply friction impulse on the ground + groundObject->applyImpulse(-sideImp,rel_pos2); + } + } + } + + +} + + +void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) +{ +// RayResultCallback& resultCallback; + + btCollisionWorld::ClosestRayResultCallback rayCallback(from,to); + + m_dynamicsWorld->rayTest(from, to, rayCallback); + + if (rayCallback.hasHit()) + { + + btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject); + if (body && body->hasContactResponse()) + { + result.m_hitPointInWorld = rayCallback.m_hitPointWorld; + result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld; + result.m_hitNormalInWorld.normalize(); + result.m_distFraction = rayCallback.m_closestHitFraction; + return body; + } + } + return 0; +} diff --git a/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/src/BulletDynamics/Vehicle/btWheelInfo.cpp index e51c0a5f1..ef93c16ff 100644 --- a/src/BulletDynamics/Vehicle/btWheelInfo.cpp +++ b/src/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -1,56 +1,56 @@ -/* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies. - * Erwin Coumans makes no representations about the suitability - * of this software for any purpose. - * It is provided "as is" without express or implied warranty. -*/ -#include "btWheelInfo.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity - - -btScalar btWheelInfo::getSuspensionRestLength() const -{ - - return m_suspensionRestLength1; - -} - -void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) -{ - (void)raycastInfo; - - - if (m_raycastInfo.m_isInContact) - - { - btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); - btVector3 chassis_velocity_at_contactPoint; - btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); - chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); - btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); - if ( project >= btScalar(-0.1)) - { - m_suspensionRelativeVelocity = btScalar(0.0); - m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); - } - else - { - btScalar inv = btScalar(-1.) / project; - m_suspensionRelativeVelocity = projVel * inv; - m_clippedInvContactDotSuspension = inv; - } - - } - - else // Not in contact : position wheel in a nice (rest length) position - { - m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); - m_suspensionRelativeVelocity = btScalar(0.0); - m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; - m_clippedInvContactDotSuspension = btScalar(1.0); - } -} +/* + * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * Permission to use, copy, modify, distribute and sell this software + * and its documentation for any purpose is hereby granted without fee, + * provided that the above copyright notice appear in all copies. + * Erwin Coumans makes no representations about the suitability + * of this software for any purpose. + * It is provided "as is" without express or implied warranty. +*/ +#include "btWheelInfo.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity + + +btScalar btWheelInfo::getSuspensionRestLength() const +{ + + return m_suspensionRestLength1; + +} + +void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) +{ + (void)raycastInfo; + + + if (m_raycastInfo.m_isInContact) + + { + btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); + btVector3 chassis_velocity_at_contactPoint; + btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); + chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); + btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + if ( project >= btScalar(-0.1)) + { + m_suspensionRelativeVelocity = btScalar(0.0); + m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); + } + else + { + btScalar inv = btScalar(-1.) / project; + m_suspensionRelativeVelocity = projVel * inv; + m_clippedInvContactDotSuspension = inv; + } + + } + + else // Not in contact : position wheel in a nice (rest length) position + { + m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); + m_suspensionRelativeVelocity = btScalar(0.0); + m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + m_clippedInvContactDotSuspension = btScalar(1.0); + } +} diff --git a/src/BulletMultiThreaded/SequentialThreadSupport.cpp b/src/BulletMultiThreaded/SequentialThreadSupport.cpp index a1a473aeb..4e9c822bb 100644 --- a/src/BulletMultiThreaded/SequentialThreadSupport.cpp +++ b/src/BulletMultiThreaded/SequentialThreadSupport.cpp @@ -1,93 +1,93 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SequentialThreadSupport.h" - - -#include "SpuCollisionTaskProcess.h" -#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" - -SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo) -{ - startThreads(threadConstructionInfo); -} - -///cleanup/shutdown Libspe2 -SequentialThreadSupport::~SequentialThreadSupport() -{ - stopSPU(); -} - -#include - -///send messages to SPUs -void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) -{ - switch (uiCommand) - { - case CMD_GATHER_AND_PROCESS_PAIRLIST: - { - btSpuStatus& spuStatus = m_activeSpuStatus[0]; - spuStatus.m_userPtr=(void*)uiArgument0; - spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); - } - break; - default: - { - ///not implemented - btAssert(0 && "Not implemented"); - } - - }; - - -} - -///check for messages from SPUs -void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1) -{ - btAssert(m_activeSpuStatus.size()); - btSpuStatus& spuStatus = m_activeSpuStatus[0]; - *puiArgument0 = spuStatus.m_taskId; - *puiArgument1 = spuStatus.m_status; -} - -void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo) -{ - m_activeSpuStatus.resize(1); - printf("STS: Not starting any threads\n"); - btSpuStatus& spuStatus = m_activeSpuStatus[0]; - spuStatus.m_userPtr = 0; - spuStatus.m_taskId = 0; - spuStatus.m_commandId = 0; - spuStatus.m_status = 0; - spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc(); - spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; - printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName); -} - -void SequentialThreadSupport::startSPU() -{ -} - -void SequentialThreadSupport::stopSPU() -{ - m_activeSpuStatus.clear(); -} - -void SequentialThreadSupport::setNumTasks(int numTasks) -{ - printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SequentialThreadSupport.h" + + +#include "SpuCollisionTaskProcess.h" +#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" + +SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo) +{ + startThreads(threadConstructionInfo); +} + +///cleanup/shutdown Libspe2 +SequentialThreadSupport::~SequentialThreadSupport() +{ + stopSPU(); +} + +#include + +///send messages to SPUs +void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) +{ + switch (uiCommand) + { + case CMD_GATHER_AND_PROCESS_PAIRLIST: + { + btSpuStatus& spuStatus = m_activeSpuStatus[0]; + spuStatus.m_userPtr=(void*)uiArgument0; + spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); + } + break; + default: + { + ///not implemented + btAssert(0 && "Not implemented"); + } + + }; + + +} + +///check for messages from SPUs +void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1) +{ + btAssert(m_activeSpuStatus.size()); + btSpuStatus& spuStatus = m_activeSpuStatus[0]; + *puiArgument0 = spuStatus.m_taskId; + *puiArgument1 = spuStatus.m_status; +} + +void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo) +{ + m_activeSpuStatus.resize(1); + printf("STS: Not starting any threads\n"); + btSpuStatus& spuStatus = m_activeSpuStatus[0]; + spuStatus.m_userPtr = 0; + spuStatus.m_taskId = 0; + spuStatus.m_commandId = 0; + spuStatus.m_status = 0; + spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc(); + spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; + printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName); +} + +void SequentialThreadSupport::startSPU() +{ +} + +void SequentialThreadSupport::stopSPU() +{ + m_activeSpuStatus.clear(); +} + +void SequentialThreadSupport::setNumTasks(int numTasks) +{ + printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks); +} diff --git a/src/BulletMultiThreaded/SequentialThreadSupport.h b/src/BulletMultiThreaded/SequentialThreadSupport.h index 67f7f2e6b..031de7ea9 100644 --- a/src/BulletMultiThreaded/SequentialThreadSupport.h +++ b/src/BulletMultiThreaded/SequentialThreadSupport.h @@ -1,87 +1,87 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btScalar.h" -#include "PlatformDefinitions.h" - - -#ifndef SEQUENTIAL_THREAD_SUPPORT_H -#define SEQUENTIAL_THREAD_SUPPORT_H - -#include "LinearMath/btAlignedObjectArray.h" - -#include "btThreadSupportInterface.h" - -typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory); -typedef void* (*SequentiallsMemorySetupFunc)(); - - - -///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface -///This is useful for debugging and porting SPU Tasks to other platforms. -class SequentialThreadSupport : public btThreadSupportInterface -{ -public: - struct btSpuStatus - { - uint32_t m_taskId; - uint32_t m_commandId; - uint32_t m_status; - - SequentialThreadFunc m_userThreadFunc; - - void* m_userPtr; //for taskDesc etc - void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc - }; -private: - btAlignedObjectArray m_activeSpuStatus; - btAlignedObjectArray m_completeHandles; -public: - struct SequentialThreadConstructionInfo - { - SequentialThreadConstructionInfo (char* uniqueName, - SequentialThreadFunc userThreadFunc, - SequentiallsMemorySetupFunc lsMemoryFunc - ) - :m_uniqueName(uniqueName), - m_userThreadFunc(userThreadFunc), - m_lsMemoryFunc(lsMemoryFunc) - { - - } - - char* m_uniqueName; - SequentialThreadFunc m_userThreadFunc; - SequentiallsMemorySetupFunc m_lsMemoryFunc; - }; - - SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo); - virtual ~SequentialThreadSupport(); - void startThreads(SequentialThreadConstructionInfo& threadInfo); -///send messages to SPUs - virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); -///check for messages from SPUs - virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); -///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded) - virtual void startSPU(); -///tell the task scheduler we are done with the SPU tasks - virtual void stopSPU(); - - virtual void setNumTasks(int numTasks); - -}; - -#endif //SEQUENTIAL_THREAD_SUPPORT_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btScalar.h" +#include "PlatformDefinitions.h" + + +#ifndef SEQUENTIAL_THREAD_SUPPORT_H +#define SEQUENTIAL_THREAD_SUPPORT_H + +#include "LinearMath/btAlignedObjectArray.h" + +#include "btThreadSupportInterface.h" + +typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory); +typedef void* (*SequentiallsMemorySetupFunc)(); + + + +///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface +///This is useful for debugging and porting SPU Tasks to other platforms. +class SequentialThreadSupport : public btThreadSupportInterface +{ +public: + struct btSpuStatus + { + uint32_t m_taskId; + uint32_t m_commandId; + uint32_t m_status; + + SequentialThreadFunc m_userThreadFunc; + + void* m_userPtr; //for taskDesc etc + void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc + }; +private: + btAlignedObjectArray m_activeSpuStatus; + btAlignedObjectArray m_completeHandles; +public: + struct SequentialThreadConstructionInfo + { + SequentialThreadConstructionInfo (char* uniqueName, + SequentialThreadFunc userThreadFunc, + SequentiallsMemorySetupFunc lsMemoryFunc + ) + :m_uniqueName(uniqueName), + m_userThreadFunc(userThreadFunc), + m_lsMemoryFunc(lsMemoryFunc) + { + + } + + char* m_uniqueName; + SequentialThreadFunc m_userThreadFunc; + SequentiallsMemorySetupFunc m_lsMemoryFunc; + }; + + SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo); + virtual ~SequentialThreadSupport(); + void startThreads(SequentialThreadConstructionInfo& threadInfo); +///send messages to SPUs + virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); +///check for messages from SPUs + virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); +///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded) + virtual void startSPU(); +///tell the task scheduler we are done with the SPU tasks + virtual void stopSPU(); + + virtual void setNumTasks(int numTasks); + +}; + +#endif //SEQUENTIAL_THREAD_SUPPORT_H + diff --git a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp index b2d93b38c..286b63191 100644 --- a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp +++ b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp @@ -1,69 +1,69 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuContactManifoldCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" - - - - -void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - btAssert(0); -} - -btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - btAssert(0); - return 1.f; -} - -#ifndef __SPU__ -SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1) -:btCollisionAlgorithm(ci) -#ifdef USE_SEPDISTANCE_UTIL -,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc()) -#endif //USE_SEPDISTANCE_UTIL -{ - m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); - m_shapeType0 = body0->getCollisionShape()->getShapeType(); - m_shapeType1 = body1->getCollisionShape()->getShapeType(); - m_collisionMargin0 = body0->getCollisionShape()->getMargin(); - m_collisionMargin1 = body1->getCollisionShape()->getMargin(); - m_collisionObject0 = body0; - m_collisionObject1 = body1; - - if (body0->getCollisionShape()->isPolyhedral()) - { - btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape(); - m_shapeDimensions0 = convex0->getImplicitShapeDimensions(); - } - if (body1->getCollisionShape()->isPolyhedral()) - { - btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape(); - m_shapeDimensions1 = convex1->getImplicitShapeDimensions(); - } -} -#endif //__SPU__ - - -SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm() -{ - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuContactManifoldCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" + + + + +void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); +} + +btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btAssert(0); + return 1.f; +} + +#ifndef __SPU__ +SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1) +:btCollisionAlgorithm(ci) +#ifdef USE_SEPDISTANCE_UTIL +,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc()) +#endif //USE_SEPDISTANCE_UTIL +{ + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_shapeType0 = body0->getCollisionShape()->getShapeType(); + m_shapeType1 = body1->getCollisionShape()->getShapeType(); + m_collisionMargin0 = body0->getCollisionShape()->getMargin(); + m_collisionMargin1 = body1->getCollisionShape()->getMargin(); + m_collisionObject0 = body0; + m_collisionObject1 = body1; + + if (body0->getCollisionShape()->isPolyhedral()) + { + btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape(); + m_shapeDimensions0 = convex0->getImplicitShapeDimensions(); + } + if (body1->getCollisionShape()->isPolyhedral()) + { + btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape(); + m_shapeDimensions1 = convex1->getImplicitShapeDimensions(); + } +} +#endif //__SPU__ + + +SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm() +{ + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); +} diff --git a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h index 486a49742..151cb2c79 100644 --- a/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h +++ b/src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h @@ -1,120 +1,120 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H -#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "LinearMath/btTransformUtil.h" - -class btPersistentManifold; - -//#define USE_SEPDISTANCE_UTIL 1 - -/// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU. -ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm -{ - btVector3 m_shapeDimensions0; - btVector3 m_shapeDimensions1; - btPersistentManifold* m_manifoldPtr; - int m_shapeType0; - int m_shapeType1; - float m_collisionMargin0; - float m_collisionMargin1; - - btCollisionObject* m_collisionObject0; - btCollisionObject* m_collisionObject1; - - - - -public: - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - - SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); -#ifdef USE_SEPDISTANCE_UTIL - btConvexSeparatingDistanceUtil m_sepDistance; -#endif //USE_SEPDISTANCE_UTIL - - virtual ~SpuContactManifoldCollisionAlgorithm(); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr) - manifoldArray.push_back(m_manifoldPtr); - } - - btPersistentManifold* getContactManifoldPtr() - { - return m_manifoldPtr; - } - - btCollisionObject* getCollisionObject0() - { - return m_collisionObject0; - } - - btCollisionObject* getCollisionObject1() - { - return m_collisionObject1; - } - - int getShapeType0() const - { - return m_shapeType0; - } - - int getShapeType1() const - { - return m_shapeType1; - } - float getCollisionMargin0() const - { - return m_collisionMargin0; - } - float getCollisionMargin1() const - { - return m_collisionMargin1; - } - - const btVector3& getShapeDimensions0() const - { - return m_shapeDimensions0; - } - - const btVector3& getShapeDimensions1() const - { - return m_shapeDimensions1; - } - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm)); - return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1); - } - }; - -}; - -#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H +#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "LinearMath/btTransformUtil.h" + +class btPersistentManifold; + +//#define USE_SEPDISTANCE_UTIL 1 + +/// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU. +ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm +{ + btVector3 m_shapeDimensions0; + btVector3 m_shapeDimensions1; + btPersistentManifold* m_manifoldPtr; + int m_shapeType0; + int m_shapeType1; + float m_collisionMargin0; + float m_collisionMargin1; + + btCollisionObject* m_collisionObject0; + btCollisionObject* m_collisionObject1; + + + + +public: + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + + SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); +#ifdef USE_SEPDISTANCE_UTIL + btConvexSeparatingDistanceUtil m_sepDistance; +#endif //USE_SEPDISTANCE_UTIL + + virtual ~SpuContactManifoldCollisionAlgorithm(); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr) + manifoldArray.push_back(m_manifoldPtr); + } + + btPersistentManifold* getContactManifoldPtr() + { + return m_manifoldPtr; + } + + btCollisionObject* getCollisionObject0() + { + return m_collisionObject0; + } + + btCollisionObject* getCollisionObject1() + { + return m_collisionObject1; + } + + int getShapeType0() const + { + return m_shapeType0; + } + + int getShapeType1() const + { + return m_shapeType1; + } + float getCollisionMargin0() const + { + return m_collisionMargin0; + } + float getCollisionMargin1() const + { + return m_collisionMargin1; + } + + const btVector3& getShapeDimensions0() const + { + return m_shapeDimensions0; + } + + const btVector3& getShapeDimensions1() const + { + return m_shapeDimensions1; + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm)); + return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1); + } + }; + +}; + +#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H diff --git a/src/BulletMultiThreaded/SpuFakeDma.cpp b/src/BulletMultiThreaded/SpuFakeDma.cpp index dc1e3c411..5b3dc345d 100644 --- a/src/BulletMultiThreaded/SpuFakeDma.cpp +++ b/src/BulletMultiThreaded/SpuFakeDma.cpp @@ -1,209 +1,209 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuFakeDma.h" -#include //for btAssert -//Disabling memcpy sometimes helps debugging DMA - -#define USE_MEMCPY 1 -#ifdef USE_MEMCPY - -#endif - - -void* cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) -{ - -#if defined (__SPU__) || defined (USE_LIBSPE2) - cellDmaLargeGet(ls,ea,size,tag,tid,rid); - return ls; -#else - return (void*)(uint32_t)ea; -#endif -} - -void* cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) -{ -#if defined (__SPU__) || defined (USE_LIBSPE2) - mfc_get(ls,ea,size,tag,0,0); - return ls; -#else - return (void*)(uint32_t)ea; -#endif -} - - - - -void* cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) -{ -#if defined (__SPU__) || defined (USE_LIBSPE2) - cellDmaGet(ls,ea,size,tag,tid,rid); - return ls; -#else - return (void*)(uint32_t)ea; -#endif -} - - -///this unalignedDma should not be frequently used, only for small data. It handles alignment and performs check on size (<16 bytes) -int stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size) -{ - - btAssert(size<32); - - ATTRIBUTE_ALIGNED16(char tmpBuffer[32]); - - char* mainMem = (char*)ea; - char* localStore = (char*)ls; - uint32_t i; - - - ///make sure last 4 bits are the same, for cellDmaSmallGet - uint32_t last4BitsOffset = ea & 0x0f; - char* tmpTarget = tmpBuffer + last4BitsOffset; - -#if defined (__SPU__) || defined (USE_LIBSPE2) - - int remainingSize = size; - -//#define FORCE_cellDmaUnalignedGet 1 -#ifdef FORCE_cellDmaUnalignedGet - cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0); -#else - char* remainingTmpTarget = tmpTarget; - uint64_t remainingEa = ea; - - while (remainingSize) - { - switch (remainingSize) - { - case 1: - case 2: - case 4: - case 8: - case 16: - { - mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0); - remainingSize=0; - break; - } - default: - { - //spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize); - int actualSize = 0; - - if (remainingSize > 16) - actualSize = 16; - else - if (remainingSize >8) - actualSize=8; - else - if (remainingSize >4) - actualSize=4; - else - if (remainingSize >2) - actualSize=2; - mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0); - remainingSize-=actualSize; - remainingTmpTarget+=actualSize; - remainingEa += actualSize; - } - } - } -#endif//FORCE_cellDmaUnalignedGet - -#else - //copy into final destination -#ifdef USE_MEMCPY - memcpy(tmpTarget,mainMem,size); -#else - for ( i=0;i //for btAssert +//Disabling memcpy sometimes helps debugging DMA + +#define USE_MEMCPY 1 +#ifdef USE_MEMCPY + +#endif + + +void* cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) +{ + +#if defined (__SPU__) || defined (USE_LIBSPE2) + cellDmaLargeGet(ls,ea,size,tag,tid,rid); + return ls; +#else + return (void*)(uint32_t)ea; +#endif +} + +void* cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) +{ +#if defined (__SPU__) || defined (USE_LIBSPE2) + mfc_get(ls,ea,size,tag,0,0); + return ls; +#else + return (void*)(uint32_t)ea; +#endif +} + + + + +void* cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) +{ +#if defined (__SPU__) || defined (USE_LIBSPE2) + cellDmaGet(ls,ea,size,tag,tid,rid); + return ls; +#else + return (void*)(uint32_t)ea; +#endif +} + + +///this unalignedDma should not be frequently used, only for small data. It handles alignment and performs check on size (<16 bytes) +int stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size) +{ + + btAssert(size<32); + + ATTRIBUTE_ALIGNED16(char tmpBuffer[32]); + + char* mainMem = (char*)ea; + char* localStore = (char*)ls; + uint32_t i; + + + ///make sure last 4 bits are the same, for cellDmaSmallGet + uint32_t last4BitsOffset = ea & 0x0f; + char* tmpTarget = tmpBuffer + last4BitsOffset; + +#if defined (__SPU__) || defined (USE_LIBSPE2) + + int remainingSize = size; + +//#define FORCE_cellDmaUnalignedGet 1 +#ifdef FORCE_cellDmaUnalignedGet + cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0); +#else + char* remainingTmpTarget = tmpTarget; + uint64_t remainingEa = ea; + + while (remainingSize) + { + switch (remainingSize) + { + case 1: + case 2: + case 4: + case 8: + case 16: + { + mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0); + remainingSize=0; + break; + } + default: + { + //spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize); + int actualSize = 0; + + if (remainingSize > 16) + actualSize = 16; + else + if (remainingSize >8) + actualSize=8; + else + if (remainingSize >4) + actualSize=4; + else + if (remainingSize >2) + actualSize=2; + mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0); + remainingSize-=actualSize; + remainingTmpTarget+=actualSize; + remainingEa += actualSize; + } + } + } +#endif//FORCE_cellDmaUnalignedGet + +#else + //copy into final destination +#ifdef USE_MEMCPY + memcpy(tmpTarget,mainMem,size); +#else + for ( i=0;i -#include - -using namespace Vectormath::Aos; - -enum FeatureType { F, E, V }; - -//---------------------------------------------------------------------------- -// Box -//---------------------------------------------------------------------------- -///The Box is an internal class used by the boxBoxDistance calculation. -class Box -{ -public: - Vector3 half; - - inline Box() - {} - inline Box(PE_REF(Vector3) half_); - inline Box(float hx, float hy, float hz); - - inline void Set(PE_REF(Vector3) half_); - inline void Set(float hx, float hy, float hz); - - inline Vector3 GetAABB(const Matrix3& rotation) const; -}; - -inline -Box::Box(PE_REF(Vector3) half_) -{ - Set(half_); -} - -inline -Box::Box(float hx, float hy, float hz) -{ - Set(hx, hy, hz); -} - -inline -void -Box::Set(PE_REF(Vector3) half_) -{ - half = half_; -} - -inline -void -Box::Set(float hx, float hy, float hz) -{ - half = Vector3(hx, hy, hz); -} - -inline -Vector3 -Box::GetAABB(const Matrix3& rotation) const -{ - return absPerElem(rotation) * half; -} - -//------------------------------------------------------------------------------------------------- -// BoxPoint -//------------------------------------------------------------------------------------------------- - -///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation. -class BoxPoint -{ -public: - BoxPoint() : localPoint(0.0f) {} - - Point3 localPoint; - FeatureType featureType; - int featureIdx; - - inline void setVertexFeature(int plusX, int plusY, int plusZ); - inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1); - inline void setFaceFeature(int dim, int plus); - - inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const; - inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const; - inline void getFaceFeature(int & dim, int & plus) const; -}; - -inline -void -BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ) -{ - featureType = V; - featureIdx = plusX << 2 | plusY << 1 | plusZ; -} - -inline -void -BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1) -{ - featureType = E; - - if (dim0 > dim1) { - featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0; - } else { - featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1; - } -} - -inline -void -BoxPoint::setFaceFeature(int dim, int plus) -{ - featureType = F; - featureIdx = plus << 2 | dim; -} - -inline -void -BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const -{ - plusX = featureIdx >> 2; - plusY = featureIdx >> 1 & 1; - plusZ = featureIdx & 1; -} - -inline -void -BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const -{ - plus0 = featureIdx >> 5; - dim0 = featureIdx >> 3 & 3; - plus1 = featureIdx >> 2 & 1; - dim1 = featureIdx & 3; -} - -inline -void -BoxPoint::getFaceFeature(int & dim, int & plus) const -{ - plus = featureIdx >> 2; - dim = featureIdx & 3; -} - -#endif /* __BOX_H__ */ +/* + Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. + All rights reserved. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#ifndef __BOX_H__ +#define __BOX_H__ + + +#ifndef PE_REF +#define PE_REF(a) a& +#endif + +#include +#include + +using namespace Vectormath::Aos; + +enum FeatureType { F, E, V }; + +//---------------------------------------------------------------------------- +// Box +//---------------------------------------------------------------------------- +///The Box is an internal class used by the boxBoxDistance calculation. +class Box +{ +public: + Vector3 half; + + inline Box() + {} + inline Box(PE_REF(Vector3) half_); + inline Box(float hx, float hy, float hz); + + inline void Set(PE_REF(Vector3) half_); + inline void Set(float hx, float hy, float hz); + + inline Vector3 GetAABB(const Matrix3& rotation) const; +}; + +inline +Box::Box(PE_REF(Vector3) half_) +{ + Set(half_); +} + +inline +Box::Box(float hx, float hy, float hz) +{ + Set(hx, hy, hz); +} + +inline +void +Box::Set(PE_REF(Vector3) half_) +{ + half = half_; +} + +inline +void +Box::Set(float hx, float hy, float hz) +{ + half = Vector3(hx, hy, hz); +} + +inline +Vector3 +Box::GetAABB(const Matrix3& rotation) const +{ + return absPerElem(rotation) * half; +} + +//------------------------------------------------------------------------------------------------- +// BoxPoint +//------------------------------------------------------------------------------------------------- + +///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation. +class BoxPoint +{ +public: + BoxPoint() : localPoint(0.0f) {} + + Point3 localPoint; + FeatureType featureType; + int featureIdx; + + inline void setVertexFeature(int plusX, int plusY, int plusZ); + inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1); + inline void setFaceFeature(int dim, int plus); + + inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const; + inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const; + inline void getFaceFeature(int & dim, int & plus) const; +}; + +inline +void +BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ) +{ + featureType = V; + featureIdx = plusX << 2 | plusY << 1 | plusZ; +} + +inline +void +BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1) +{ + featureType = E; + + if (dim0 > dim1) { + featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0; + } else { + featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1; + } +} + +inline +void +BoxPoint::setFaceFeature(int dim, int plus) +{ + featureType = F; + featureIdx = plus << 2 | dim; +} + +inline +void +BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const +{ + plusX = featureIdx >> 2; + plusY = featureIdx >> 1 & 1; + plusZ = featureIdx & 1; +} + +inline +void +BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const +{ + plus0 = featureIdx >> 5; + dim0 = featureIdx >> 3 & 3; + plus1 = featureIdx >> 2 & 1; + dim1 = featureIdx & 3; +} + +inline +void +BoxPoint::getFaceFeature(int & dim, int & plus) const +{ + plus = featureIdx >> 2; + dim = featureIdx & 3; +} + +#endif /* __BOX_H__ */ diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp index c38366a98..ab8f3c259 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp @@ -1,542 +1,542 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "SpuCollisionShapes.h" - -///not supported on IBM SDK, until we fix the alignment of btVector3 -#if defined (__CELLOS_LV2__) && defined (__SPU__) -#include -static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) -{ - vec_float4 result; - result = spu_mul( vec0, vec1 ); - result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); - return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); -} -#endif //__SPU__ - -btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) -{ - switch (shapeType) - { - case SPHERE_SHAPE_PROXYTYPE: - { - return btVector3(0,0,0); - } - case BOX_SHAPE_PROXYTYPE: - { -// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); - btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; - const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); - - return btVector3( - localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), - localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), - localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); - } - - case TRIANGLE_SHAPE_PROXYTYPE: - { - - btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3* vertices = (btVector3*)shape; - btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); - btVector3 sup = vertices[dots.maxAxis()]; - return btVector3(sup.getX(),sup.getY(),sup.getZ()); - break; - } - - case CYLINDER_SHAPE_PROXYTYPE: - { - btCylinderShape* cylShape = (btCylinderShape*)shape; - - //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) - - btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); - btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); - - int cylinderUpAxis = cylShape->getUpAxis(); - int XX(1),YY(0),ZZ(2); - - switch (cylinderUpAxis) - { - case 0: - { - XX = 1; - YY = 0; - ZZ = 2; - break; - } - case 1: - { - XX = 0; - YY = 1; - ZZ = 2; - break; - } - case 2: - { - XX = 0; - YY = 2; - ZZ = 1; - break; - } - default: - btAssert(0); - //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); - }; - - btScalar radius = halfExtents[XX]; - btScalar halfHeight = halfExtents[cylinderUpAxis]; - - btVector3 tmp; - btScalar d ; - - btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); - if (s != btScalar(0.0)) - { - d = radius / s; - tmp[XX] = v[XX] * d; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = v[ZZ] * d; - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - else - { - tmp[XX] = radius; - tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; - tmp[ZZ] = btScalar(0.0); - return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); - } - } - - case CAPSULE_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - - btCapsuleShape* capsuleShape = (btCapsuleShape*)shape; - btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - int capsuleUpAxis = capsuleShape->getUpAxis(); - - btScalar radius = capsuleShape->getRadius(); - btVector3 supVec(0,0,0); - - btScalar maxDot(btScalar(-1e30)); - - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - btVector3 vtx; - btScalar newDot; - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = halfHeight; - - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - { - btVector3 pos(0,0,0); - pos[capsuleUpAxis] = -halfHeight; - - vtx = pos +vec*(radius); - newDot = vec.dot(vtx); - if (newDot > maxDot) - { - maxDot = newDot; - supVec = vtx; - } - } - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - break; - }; - - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); - -#if defined (__CELLOS_LV2__) && defined (__SPU__) - vec_float4 v_distMax = {-FLT_MAX,0,0,0}; - vec_int4 v_idxMax = {-999,0,0,0}; - int v=0; - int numverts = convexVertexData->gNumConvexPoints; - btVector3* points = convexVertexData->gConvexPoints; - - for(;v<(int)numverts-4;v+=4) { - vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); - vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); - vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); - vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); - const vec_int4 i0 = {v ,0,0,0}; - const vec_int4 i1 = {v+1,0,0,0}; - const vec_int4 i2 = {v+2,0,0,0}; - const vec_int4 i3 = {v+3,0,0,0}; - vec_uint4 retGt01 = spu_cmpgt(p0,p1); - vec_float4 pmax01 = spu_sel(p1,p0,retGt01); - vec_int4 imax01 = spu_sel(i1,i0,retGt01); - vec_uint4 retGt23 = spu_cmpgt(p2,p3); - vec_float4 pmax23 = spu_sel(p3,p2,retGt23); - vec_int4 imax23 = spu_sel(i3,i2,retGt23); - vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); - vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); - vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); - vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); - v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); - v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); - } - for(;v<(int)numverts;v++) { - vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); - const vec_int4 i = {v,0,0,0}; - vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); - v_distMax = spu_sel(p,v_distMax,retGtMax); - v_idxMax = spu_sel(i,v_idxMax,retGtMax); - } - int ptIndex = spu_extract(v_idxMax,0); - const btVector3& supVec= points[ptIndex]; -#else - - btVector3* points = 0; - int numPoints = 0; - points = convexVertexData->gConvexPoints; - numPoints = convexVertexData->gNumConvexPoints; - - // spu_printf("numPoints = %d\n",numPoints); - - int ptIndex = 0; - btScalar newDot,maxDot = btScalar(-1e30); - - btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); - btVector3 vec = vec0; - btScalar lenSqr = vec.length2(); - if (lenSqr < btScalar(0.0001)) - { - vec.setValue(1,0,0); - } else - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - vec *= rlen; - } - - - for (int i=0;i maxDot) - { - maxDot = newDot; - ptIndex = i; - } - } - const btVector3& supVec= points[ptIndex]; - -#endif - return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); - - break; - }; - - default: - - //spu_printf("SPU:(type %i) missing support function\n",shapeType); - - -#if __ASSERT - // spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); -#endif // __ASSERT - return btVector3(0.f, 0.f, 0.f); - } -} - -void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform) -{ - //calculate the aabb, given the types... - switch (shapeType) - { - case CYLINDER_SHAPE_PROXYTYPE: - /* fall through */ - case BOX_SHAPE_PROXYTYPE: - { - btScalar margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - halfExtents += btVector3(margin,margin,margin); - const btTransform& t = xform; - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - btScalar margin=convexShape->getMarginNV(); - btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); - //add the radius to y-axis to get full height - btScalar radius = halfExtents[0]; - halfExtents[1] += radius; - halfExtents += btVector3(margin,margin,margin); -#if 0 - int capsuleUpAxis = convexShape->getUpAxis(); - btScalar halfHeight = convexShape->getHalfHeight(); - btScalar radius = convexShape->getRadius(); - halfExtents[capsuleUpAxis] = radius + halfHeight; -#endif - const btTransform& t = xform; - btMatrix3x3 abs_b = t.getBasis().absolute(); - btVector3 center = t.getOrigin(); - btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); - - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case SPHERE_SHAPE_PROXYTYPE: - { - btScalar radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); - btScalar margin = radius + convexShape->getMarginNV(); - const btTransform& t = xform; - const btVector3& center = t.getOrigin(); - btVector3 extent(margin,margin,margin); - aabbMin = center - extent; - aabbMax = center + extent; - break; - } - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); - cellDmaGet(&convexHullShape0, convexShapePtr , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; - const btTransform& t = xform; - btScalar margin = convexShape->getMarginNV(); - localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin); - //spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); - //spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); - break; - } - default: - { - // spu_printf("SPU: unsupported shapetype %d in AABB calculation\n"); - } - }; -} - -void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - - dmaSize = sizeof(btTriangleIndexVertexArray); - dmaPpuAddress2 = reinterpret_cast(triMeshShape->getMeshInterface()); - // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); -#ifdef __SPU__ - cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage; -#else - bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); -#endif - - //cellDmaWaitTagStatusAll(DMA_MASK(1)); - - ///now DMA over the BVH - - dmaSize = sizeof(btOptimizedBvh); - dmaPpuAddress2 = reinterpret_cast(triMeshShape->getOptimizedBvh()); - //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2); - cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(2)); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); -} - -void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag) -{ - cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index] , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0); - -} - -void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag) -{ - cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0); -} - -void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag) -{ - cellDmaGet(nodes, reinterpret_cast(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0); -} - -///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck -int getShapeTypeSize(int shapeType) -{ - - - switch (shapeType) - { - case CYLINDER_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCylinderShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case BOX_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btBoxShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case SPHERE_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btSphereShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case TRIANGLE_MESH_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btBvhTriangleMeshShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - case CAPSULE_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCapsuleShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - - case CONVEX_HULL_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btConvexHullShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - - case COMPOUND_SHAPE_PROXYTYPE: - { - int shapeSize = sizeof(btCompoundShape); - btAssert(shapeSize < MAX_SHAPE_SIZE); - return shapeSize; - } - - default: - btAssert(0); - //unsupported shapetype, please add here - return 0; - } -} - -void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU) -{ - convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints(); - if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS) - { - btAssert(0); - // spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints); - return; - } - - register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btVector3); - ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getUnscaledPoints(); - cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU , dmaSize, DMA_TAG(2), 0, 0); -} - -void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType) -{ - register int dmaSize = getShapeTypeSize(shapeType); - cellDmaGet(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(dmaTag)); -} - -void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - int childShapeCount = spuCompoundShape->getNumChildShapes(); - dmaSize = childShapeCount * sizeof(btCompoundShapeChild); - dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList(); - cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0); -} - -void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) -{ - int childShapeCount = spuCompoundShape->getNumChildShapes(); - int i; - // DMA all the subshapes - for ( i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i]; - dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType); - } -} - - -void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex) -{ - - int curIndex = startNodeIndex; - int walkIterations = 0; -#ifdef BT_DEBUG - int subTreeSize = endNodeIndex - startNodeIndex; -#endif - - int escapeIndex; - - unsigned int aabbOverlap, isLeafNode; - - while (curIndex < endNodeIndex) - { - //catch bugs in tree data - btAssert (walkIterations < subTreeSize); - - walkIterations++; - aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - isLeafNode = rootNode->isLeafNode(); - - if (isLeafNode && aabbOverlap) - { - //printf("overlap with node %d\n",rootNode->getTriangleIndex()); - nodeCallback->processNode(0,rootNode->getTriangleIndex()); - // spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex()); - } - - if (aabbOverlap || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "SpuCollisionShapes.h" + +///not supported on IBM SDK, until we fix the alignment of btVector3 +#if defined (__CELLOS_LV2__) && defined (__SPU__) +#include +static inline vec_float4 vec_dot3( vec_float4 vec0, vec_float4 vec1 ) +{ + vec_float4 result; + result = spu_mul( vec0, vec1 ); + result = spu_madd( spu_rlqwbyte( vec0, 4 ), spu_rlqwbyte( vec1, 4 ), result ); + return spu_madd( spu_rlqwbyte( vec0, 8 ), spu_rlqwbyte( vec1, 8 ), result ); +} +#endif //__SPU__ + +btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData)//, int *featureIndex) +{ + switch (shapeType) + { + case SPHERE_SHAPE_PROXYTYPE: + { + return btVector3(0,0,0); + } + case BOX_SHAPE_PROXYTYPE: + { +// spu_printf("SPU: getSupport BOX_SHAPE_PROXYTYPE\n"); + btConvexInternalShape* convexShape = (btConvexInternalShape*)shape; + const btVector3& halfExtents = convexShape->getImplicitShapeDimensions(); + + return btVector3( + localDir.getX() < 0.0f ? -halfExtents.x() : halfExtents.x(), + localDir.getY() < 0.0f ? -halfExtents.y() : halfExtents.y(), + localDir.getZ() < 0.0f ? -halfExtents.z() : halfExtents.z()); + } + + case TRIANGLE_SHAPE_PROXYTYPE: + { + + btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3* vertices = (btVector3*)shape; + btVector3 dots(dir.dot(vertices[0]), dir.dot(vertices[1]), dir.dot(vertices[2])); + btVector3 sup = vertices[dots.maxAxis()]; + return btVector3(sup.getX(),sup.getY(),sup.getZ()); + break; + } + + case CYLINDER_SHAPE_PROXYTYPE: + { + btCylinderShape* cylShape = (btCylinderShape*)shape; + + //mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis) + + btVector3 halfExtents = cylShape->getImplicitShapeDimensions(); + btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ()); + + int cylinderUpAxis = cylShape->getUpAxis(); + int XX(1),YY(0),ZZ(2); + + switch (cylinderUpAxis) + { + case 0: + { + XX = 1; + YY = 0; + ZZ = 2; + break; + } + case 1: + { + XX = 0; + YY = 1; + ZZ = 2; + break; + } + case 2: + { + XX = 0; + YY = 2; + ZZ = 1; + break; + } + default: + btAssert(0); + //printf("SPU:localGetSupportingVertexWithoutMargin unknown Cylinder up-axis\n"); + }; + + btScalar radius = halfExtents[XX]; + btScalar halfHeight = halfExtents[cylinderUpAxis]; + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return btVector3(tmp.getX(),tmp.getY(),tmp.getZ()); + } + } + + case CAPSULE_SHAPE_PROXYTYPE: + { + //spu_printf("SPU: todo: getSupport CAPSULE_SHAPE_PROXYTYPE\n"); + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + + btCapsuleShape* capsuleShape = (btCapsuleShape*)shape; + btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + int capsuleUpAxis = capsuleShape->getUpAxis(); + + btScalar radius = capsuleShape->getRadius(); + btVector3 supVec(0,0,0); + + btScalar maxDot(btScalar(-1e30)); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + btVector3 vtx; + btScalar newDot; + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = halfHeight; + + vtx = pos +vec*(radius); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + { + btVector3 pos(0,0,0); + pos[capsuleUpAxis] = -halfHeight; + + vtx = pos +vec*(radius); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); + break; + }; + + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + //spu_printf("SPU: todo: getSupport CONVEX_HULL_SHAPE_PROXYTYPE\n"); + +#if defined (__CELLOS_LV2__) && defined (__SPU__) + vec_float4 v_distMax = {-FLT_MAX,0,0,0}; + vec_int4 v_idxMax = {-999,0,0,0}; + int v=0; + int numverts = convexVertexData->gNumConvexPoints; + btVector3* points = convexVertexData->gConvexPoints; + + for(;v<(int)numverts-4;v+=4) { + vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128()); + vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128()); + vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128()); + vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128()); + const vec_int4 i0 = {v ,0,0,0}; + const vec_int4 i1 = {v+1,0,0,0}; + const vec_int4 i2 = {v+2,0,0,0}; + const vec_int4 i3 = {v+3,0,0,0}; + vec_uint4 retGt01 = spu_cmpgt(p0,p1); + vec_float4 pmax01 = spu_sel(p1,p0,retGt01); + vec_int4 imax01 = spu_sel(i1,i0,retGt01); + vec_uint4 retGt23 = spu_cmpgt(p2,p3); + vec_float4 pmax23 = spu_sel(p3,p2,retGt23); + vec_int4 imax23 = spu_sel(i3,i2,retGt23); + vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23); + vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123); + vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123); + vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123); + v_distMax = spu_sel(pmax0123,v_distMax,retGtMax); + v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax); + } + for(;v<(int)numverts;v++) { + vec_float4 p = vec_dot3(points[v].get128(),localDir.get128()); + const vec_int4 i = {v,0,0,0}; + vec_uint4 retGtMax = spu_cmpgt(v_distMax,p); + v_distMax = spu_sel(p,v_distMax,retGtMax); + v_idxMax = spu_sel(i,v_idxMax,retGtMax); + } + int ptIndex = spu_extract(v_idxMax,0); + const btVector3& supVec= points[ptIndex]; +#else + + btVector3* points = 0; + int numPoints = 0; + points = convexVertexData->gConvexPoints; + numPoints = convexVertexData->gNumConvexPoints; + + // spu_printf("numPoints = %d\n",numPoints); + + int ptIndex = 0; + btScalar newDot,maxDot = btScalar(-1e30); + + btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ()); + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < btScalar(0.0001)) + { + vec.setValue(1,0,0); + } else + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (int i=0;i maxDot) + { + maxDot = newDot; + ptIndex = i; + } + } + const btVector3& supVec= points[ptIndex]; + +#endif + return btVector3(supVec.getX(),supVec.getY(),supVec.getZ()); + + break; + }; + + default: + + //spu_printf("SPU:(type %i) missing support function\n",shapeType); + + +#if __ASSERT + // spu_printf("localGetSupportingVertexWithoutMargin() - Unsupported bound type: %d.\n", shapeType); +#endif // __ASSERT + return btVector3(0.f, 0.f, 0.f); + } +} + +void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform) +{ + //calculate the aabb, given the types... + switch (shapeType) + { + case CYLINDER_SHAPE_PROXYTYPE: + /* fall through */ + case BOX_SHAPE_PROXYTYPE: + { + btScalar margin=convexShape->getMarginNV(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + halfExtents += btVector3(margin,margin,margin); + const btTransform& t = xform; + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + btScalar margin=convexShape->getMarginNV(); + btVector3 halfExtents = convexShape->getImplicitShapeDimensions(); + //add the radius to y-axis to get full height + btScalar radius = halfExtents[0]; + halfExtents[1] += radius; + halfExtents += btVector3(margin,margin,margin); +#if 0 + int capsuleUpAxis = convexShape->getUpAxis(); + btScalar halfHeight = convexShape->getHalfHeight(); + btScalar radius = convexShape->getRadius(); + halfExtents[capsuleUpAxis] = radius + halfHeight; +#endif + const btTransform& t = xform; + btMatrix3x3 abs_b = t.getBasis().absolute(); + btVector3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); + + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case SPHERE_SHAPE_PROXYTYPE: + { + btScalar radius = convexShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX(); + btScalar margin = radius + convexShape->getMarginNV(); + const btTransform& t = xform; + const btVector3& center = t.getOrigin(); + btVector3 extent(margin,margin,margin); + aabbMin = center - extent; + aabbMax = center + extent; + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); + cellDmaGet(&convexHullShape0, convexShapePtr , sizeof(btConvexHullShape), DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + btConvexHullShape* localPtr = (btConvexHullShape*)&convexHullShape0; + const btTransform& t = xform; + btScalar margin = convexShape->getMarginNV(); + localPtr->getNonvirtualAabb(t,aabbMin,aabbMax,margin); + //spu_printf("SPU convex aabbMin=%f,%f,%f=\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ()); + //spu_printf("SPU convex aabbMax=%f,%f,%f=\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ()); + break; + } + default: + { + // spu_printf("SPU: unsupported shapetype %d in AABB calculation\n"); + } + }; +} + +void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape) +{ + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + + dmaSize = sizeof(btTriangleIndexVertexArray); + dmaPpuAddress2 = reinterpret_cast(triMeshShape->getMeshInterface()); + // spu_printf("trimeshShape->getMeshInterface() == %llx\n",dmaPpuAddress2); +#ifdef __SPU__ + cellDmaGet(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + bvhMeshShape->gTriangleMeshInterfacePtr = &bvhMeshShape->gTriangleMeshInterfaceStorage; +#else + bvhMeshShape->gTriangleMeshInterfacePtr = (btTriangleIndexVertexArray*)cellDmaGetReadOnly(&bvhMeshShape->gTriangleMeshInterfaceStorage, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); +#endif + + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + + ///now DMA over the BVH + + dmaSize = sizeof(btOptimizedBvh); + dmaPpuAddress2 = reinterpret_cast(triMeshShape->getOptimizedBvh()); + //spu_printf("trimeshShape->getOptimizedBvh() == %llx\n",dmaPpuAddress2); + cellDmaGet(&bvhMeshShape->gOptimizedBvh, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(2)); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); +} + +void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag) +{ + cellDmaGet(IndexMesh, (ppu_address_t)&indexArray[index] , sizeof(btIndexedMesh), DMA_TAG(dmaTag), 0, 0); + +} + +void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag) +{ + cellDmaGet(subTreeHeaders, subTreePtr, batchSize * sizeof(btBvhSubtreeInfo), DMA_TAG(dmaTag), 0, 0); +} + +void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag) +{ + cellDmaGet(nodes, reinterpret_cast(&nodeArray[subtree.m_rootNodeIndex]) , subtree.m_subtreeSize* sizeof(btQuantizedBvhNode), DMA_TAG(2), 0, 0); +} + +///getShapeTypeSize could easily be optimized, but it is not likely a bottleneck +int getShapeTypeSize(int shapeType) +{ + + + switch (shapeType) + { + case CYLINDER_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCylinderShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case BOX_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btBoxShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case SPHERE_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btSphereShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case TRIANGLE_MESH_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btBvhTriangleMeshShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCapsuleShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + + case CONVEX_HULL_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btConvexHullShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + + case COMPOUND_SHAPE_PROXYTYPE: + { + int shapeSize = sizeof(btCompoundShape); + btAssert(shapeSize < MAX_SHAPE_SIZE); + return shapeSize; + } + + default: + btAssert(0); + //unsupported shapetype, please add here + return 0; + } +} + +void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU) +{ + convexVertexData->gNumConvexPoints = convexShapeSPU->getNumPoints(); + if (convexVertexData->gNumConvexPoints>MAX_NUM_SPU_CONVEX_POINTS) + { + btAssert(0); + // spu_printf("SPU: Error: MAX_NUM_SPU_CONVEX_POINTS(%d) exceeded: %d\n",MAX_NUM_SPU_CONVEX_POINTS,convexVertexData->gNumConvexPoints); + return; + } + + register int dmaSize = convexVertexData->gNumConvexPoints*sizeof(btVector3); + ppu_address_t pointsPPU = (ppu_address_t) convexShapeSPU->getUnscaledPoints(); + cellDmaGet(&convexVertexData->g_convexPointBuffer[0], pointsPPU , dmaSize, DMA_TAG(2), 0, 0); +} + +void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType) +{ + register int dmaSize = getShapeTypeSize(shapeType); + cellDmaGet(collisionShapeLocation, collisionShapePtr , dmaSize, DMA_TAG(dmaTag), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(dmaTag)); +} + +void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) +{ + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + int childShapeCount = spuCompoundShape->getNumChildShapes(); + dmaSize = childShapeCount * sizeof(btCompoundShapeChild); + dmaPpuAddress2 = (ppu_address_t)spuCompoundShape->getChildList(); + cellDmaGet(&compoundShapeLocation->gSubshapes[0], dmaPpuAddress2, dmaSize, DMA_TAG(dmaTag), 0, 0); +} + +void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag) +{ + int childShapeCount = spuCompoundShape->getNumChildShapes(); + int i; + // DMA all the subshapes + for ( i = 0; i < childShapeCount; ++i) + { + btCompoundShapeChild& childShape = compoundShapeLocation->gSubshapes[i]; + dmaCollisionShape (&compoundShapeLocation->gSubshapeShape[i],(ppu_address_t)childShape.m_childShape, dmaTag, childShape.m_childShapeType); + } +} + + +void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex) +{ + + int curIndex = startNodeIndex; + int walkIterations = 0; +#ifdef BT_DEBUG + int subTreeSize = endNodeIndex - startNodeIndex; +#endif + + int escapeIndex; + + unsigned int aabbOverlap, isLeafNode; + + while (curIndex < endNodeIndex) + { + //catch bugs in tree data + btAssert (walkIterations < subTreeSize); + + walkIterations++; + aabbOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + + if (isLeafNode && aabbOverlap) + { + //printf("overlap with node %d\n",rootNode->getTriangleIndex()); + nodeCallback->processNode(0,rootNode->getTriangleIndex()); + // spu_printf("SPU: overlap detected with triangleIndex:%d\n",rootNode->getTriangleIndex()); + } + + if (aabbOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + +} diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h index 1f91b3ef8..9adff95e3 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h @@ -1,125 +1,125 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#ifndef __SPU_COLLISION_SHAPES_H -#define __SPU_COLLISION_SHAPES_H - -#include "../SpuDoubleBuffer.h" - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" - -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" - -#define MAX_NUM_SPU_CONVEX_POINTS 128 - -struct SpuConvexPolyhedronVertexData -{ - void* gSpuConvexShapePtr; - btVector3* gConvexPoints; - int gNumConvexPoints; - ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]); -}; - -#define MAX_SHAPE_SIZE 256 - -struct CollisionShape_LocalStoreMemory -{ - ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]); -}; - -struct CompoundShape_LocalStoreMemory -{ - // Compound data -#define MAX_SPU_COMPOUND_SUBSHAPES 16 - ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]); - ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]); -}; - -struct bvhMeshShape_LocalStoreMemory -{ - //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); - ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); - btOptimizedBvh* getOptimizedBvh() - { - return (btOptimizedBvh*) gOptimizedBvh; - } - - ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage); - btTriangleIndexVertexArray* gTriangleMeshInterfacePtr; - ///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment - ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh); - #define MAX_SPU_SUBTREE_HEADERS 32 - //1024 - ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]); - ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]); -}; - - -btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex) -void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform); -void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape); -void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag); -void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag); -void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag); - -int getShapeTypeSize(int shapeType); -void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU); -void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType); -void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); -void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); - - -#define USE_BRANCHFREE_TEST 1 -#ifdef USE_BRANCHFREE_TEST -SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) -{ -#if defined(__CELLOS_LV2__) && defined (__SPU__) - vec_ushort8 vecMin = {aabbMin1[0],aabbMin2[0],aabbMin1[2],aabbMin2[2],aabbMin1[1],aabbMin2[1],0,0}; - vec_ushort8 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0}; - vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax); - return spu_extract(spu_gather(isGt),0)==0; - -#else - return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) - & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) - & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), - 1, 0); -#endif -} -#else - -SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) -{ - unsigned int overlap = 1; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap; - return overlap; -} -#endif - -void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex); - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef __SPU_COLLISION_SHAPES_H +#define __SPU_COLLISION_SHAPES_H + +#include "../SpuDoubleBuffer.h" + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" + +#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" + +#define MAX_NUM_SPU_CONVEX_POINTS 128 + +struct SpuConvexPolyhedronVertexData +{ + void* gSpuConvexShapePtr; + btVector3* gConvexPoints; + int gNumConvexPoints; + ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]); +}; + +#define MAX_SHAPE_SIZE 256 + +struct CollisionShape_LocalStoreMemory +{ + ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]); +}; + +struct CompoundShape_LocalStoreMemory +{ + // Compound data +#define MAX_SPU_COMPOUND_SUBSHAPES 16 + ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]); + ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]); +}; + +struct bvhMeshShape_LocalStoreMemory +{ + //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); + ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); + btOptimizedBvh* getOptimizedBvh() + { + return (btOptimizedBvh*) gOptimizedBvh; + } + + ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage); + btTriangleIndexVertexArray* gTriangleMeshInterfacePtr; + ///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment + ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh); + #define MAX_SPU_SUBTREE_HEADERS 32 + //1024 + ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]); + ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]); +}; + + +btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex) +void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform); +void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape); +void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag); +void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag); +void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag); + +int getShapeTypeSize(int shapeType); +void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU); +void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType); +void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); +void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); + + +#define USE_BRANCHFREE_TEST 1 +#ifdef USE_BRANCHFREE_TEST +SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ +#if defined(__CELLOS_LV2__) && defined (__SPU__) + vec_ushort8 vecMin = {aabbMin1[0],aabbMin2[0],aabbMin1[2],aabbMin2[2],aabbMin1[1],aabbMin2[1],0,0}; + vec_ushort8 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0}; + vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax); + return spu_extract(spu_gather(isGt),0)==0; + +#else + return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0); +#endif +} +#else + +SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ + unsigned int overlap = 1; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap; + return overlap; +} +#endif + +void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex); + +#endif diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp index e2537bfb5..b635b337d 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp @@ -1,231 +1,231 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuContactResult.h" - -//#define DEBUG_SPU_COLLISION_DETECTION 1 - - -SpuContactResult::SpuContactResult() -{ - m_manifoldAddress = 0; - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - SpuContactResult::~SpuContactResult() -{ - g_manifoldDmaExport.swapBuffers(); -} - - ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; -inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1) -{ - btScalar friction = friction0*friction1; - - const btScalar MAX_FRICTION = btScalar(10.); - - if (friction < -MAX_FRICTION) - friction = -MAX_FRICTION; - if (friction > MAX_FRICTION) - friction = MAX_FRICTION; - return friction; - -} - -inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1) -{ - return restitution0*restitution1; -} - - - - void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped) - { - //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress); - m_rootWorldTransform0 = worldTrans0; - m_rootWorldTransform1 = worldTrans1; - m_manifoldAddress = manifoldAddress; - m_spuManifold = spuManifold; - - m_combinedFriction = calculateCombinedFriction(friction0,friction1); - m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1); - m_isSwapped = isSwapped; - } - - void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - - } - - - - ///return true if it requires a dma transfer back -bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, - const btVector3& pointInWorld, - float depth, - btPersistentManifold* manifoldPtr, - btTransform& transA, - btTransform& transB, - btScalar combinedFriction, - btScalar combinedRestitution, - bool isSwapped) -{ - - float contactTreshold = manifoldPtr->getContactBreakingThreshold(); - - //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: contactTreshold %f\n",contactTreshold); -#endif //DEBUG_SPU_COLLISION_DETECTION - if (depth > manifoldPtr->getContactBreakingThreshold()) - return false; - - //provide inverses or just calculate? - btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform; - btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform; - - btVector3 pointA; - btVector3 localA; - btVector3 localB; - btVector3 normal; - - if (isSwapped) - { - normal = normalOnBInWorld * -1; - pointA = pointInWorld + normal * depth; - localA = transAInv(pointA ); - localB = transBInv(pointInWorld); - /*localA = transBInv(pointA ); - localB = transAInv(pointInWorld);*/ - } - else - { - normal = normalOnBInWorld; - pointA = pointInWorld + normal * depth; - localA = transAInv(pointA ); - localB = transBInv(pointInWorld); - } - - btManifoldPoint newPt(localA,localB,normal,depth); - - int insertIndex = manifoldPtr->getCacheEntry(newPt); - if (insertIndex >= 0) - { -// manifoldPtr->replaceContactPoint(newPt,insertIndex); -// return true; - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: same contact detected, nothing done\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - // This is not needed, just use the old info! saves a DMA transfer as well - } else - { - - newPt.m_combinedFriction = combinedFriction; - newPt.m_combinedRestitution = combinedRestitution; - - /* - ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred - //User can override friction and/or restitution - if (gContactAddedCallback && - //and if either of the two bodies requires custom material - ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || - (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) - { - //experimental feature info, for per-triangle material etc. - (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); - } - */ - manifoldPtr->addManifoldPoint(newPt); - return true; - - } - return false; - -} - - -void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) -{ - ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place - ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly' -#if defined (__SPU__) || defined (USE_LIBSPE2) - memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); - - g_manifoldDmaExport.swapBuffers(); - ppu_address_t mmAddr = (ppu_address_t)mmManifold; - g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9)); - // Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon? - //no, the swapBuffers does the wait -#endif -} - -void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) -{ - //spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); - -#ifdef DEBUG_SPU_COLLISION_DETECTION - // int sman = sizeof(rage::phManifold); -// spu_printf("sizeof_manifold = %i\n",sman); -#endif //DEBUG_SPU_COLLISION_DETECTION - - btPersistentManifold* localManifold = m_spuManifold; - - btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); - btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); - - //process the contact point - const bool retVal = ManifoldResultAddContactPoint(normalB, - pointWrld, - depth, - localManifold, - m_rootWorldTransform0, - m_rootWorldTransform1, - m_combinedFriction, - m_combinedRestitution, - m_isSwapped); - m_RequiresWriteBack = m_RequiresWriteBack || retVal; -} - -void SpuContactResult::flush() -{ - - if (m_spuManifold && m_spuManifold->getNumContacts()) - { - m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1); - m_RequiresWriteBack = true; - } - - - if (m_RequiresWriteBack) - { -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n"); - spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts()); - spu_printf("Manifold address: %llu\n", m_manifoldAddress); -#endif //DEBUG_SPU_COLLISION_DETECTION - // spu_printf("writeDoubleBufferedManifold\n"); - writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("SPU: Finished (Put) DMA\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - } - m_spuManifold = NULL; - m_RequiresWriteBack = false; -} - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuContactResult.h" + +//#define DEBUG_SPU_COLLISION_DETECTION 1 + + +SpuContactResult::SpuContactResult() +{ + m_manifoldAddress = 0; + m_spuManifold = NULL; + m_RequiresWriteBack = false; +} + + SpuContactResult::~SpuContactResult() +{ + g_manifoldDmaExport.swapBuffers(); +} + + ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; +inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1) +{ + btScalar friction = friction0*friction1; + + const btScalar MAX_FRICTION = btScalar(10.); + + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1) +{ + return restitution0*restitution1; +} + + + + void SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped) + { + //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress); + m_rootWorldTransform0 = worldTrans0; + m_rootWorldTransform1 = worldTrans1; + m_manifoldAddress = manifoldAddress; + m_spuManifold = spuManifold; + + m_combinedFriction = calculateCombinedFriction(friction0,friction1); + m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1); + m_isSwapped = isSwapped; + } + + void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + + } + + + + ///return true if it requires a dma transfer back +bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, + const btVector3& pointInWorld, + float depth, + btPersistentManifold* manifoldPtr, + btTransform& transA, + btTransform& transB, + btScalar combinedFriction, + btScalar combinedRestitution, + bool isSwapped) +{ + + float contactTreshold = manifoldPtr->getContactBreakingThreshold(); + + //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: contactTreshold %f\n",contactTreshold); +#endif //DEBUG_SPU_COLLISION_DETECTION + if (depth > manifoldPtr->getContactBreakingThreshold()) + return false; + + //provide inverses or just calculate? + btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform; + btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform; + + btVector3 pointA; + btVector3 localA; + btVector3 localB; + btVector3 normal; + + if (isSwapped) + { + normal = normalOnBInWorld * -1; + pointA = pointInWorld + normal * depth; + localA = transAInv(pointA ); + localB = transBInv(pointInWorld); + /*localA = transBInv(pointA ); + localB = transAInv(pointInWorld);*/ + } + else + { + normal = normalOnBInWorld; + pointA = pointInWorld + normal * depth; + localA = transAInv(pointA ); + localB = transBInv(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normal,depth); + + int insertIndex = manifoldPtr->getCacheEntry(newPt); + if (insertIndex >= 0) + { +// manifoldPtr->replaceContactPoint(newPt,insertIndex); +// return true; + +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: same contact detected, nothing done\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + // This is not needed, just use the old info! saves a DMA transfer as well + } else + { + + newPt.m_combinedFriction = combinedFriction; + newPt.m_combinedRestitution = combinedRestitution; + + /* + ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || + (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) + { + //experimental feature info, for per-triangle material etc. + (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); + } + */ + manifoldPtr->addManifoldPoint(newPt); + return true; + + } + return false; + +} + + +void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) +{ + ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place + ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly' +#if defined (__SPU__) || defined (USE_LIBSPE2) + memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); + + g_manifoldDmaExport.swapBuffers(); + ppu_address_t mmAddr = (ppu_address_t)mmManifold; + g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9)); + // Should there be any kind of wait here? What if somebody tries to use this tag again? What if we call this function again really soon? + //no, the swapBuffers does the wait +#endif +} + +void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) +{ + //spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); + +#ifdef DEBUG_SPU_COLLISION_DETECTION + // int sman = sizeof(rage::phManifold); +// spu_printf("sizeof_manifold = %i\n",sman); +#endif //DEBUG_SPU_COLLISION_DETECTION + + btPersistentManifold* localManifold = m_spuManifold; + + btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); + btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); + + //process the contact point + const bool retVal = ManifoldResultAddContactPoint(normalB, + pointWrld, + depth, + localManifold, + m_rootWorldTransform0, + m_rootWorldTransform1, + m_combinedFriction, + m_combinedRestitution, + m_isSwapped); + m_RequiresWriteBack = m_RequiresWriteBack || retVal; +} + +void SpuContactResult::flush() +{ + + if (m_spuManifold && m_spuManifold->getNumContacts()) + { + m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1); + m_RequiresWriteBack = true; + } + + + if (m_RequiresWriteBack) + { +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n"); + spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts()); + spu_printf("Manifold address: %llu\n", m_manifoldAddress); +#endif //DEBUG_SPU_COLLISION_DETECTION + // spu_printf("writeDoubleBufferedManifold\n"); + writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); +#ifdef DEBUG_SPU_COLLISION_DETECTION + spu_printf("SPU: Finished (Put) DMA\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + } + m_spuManifold = NULL; + m_RequiresWriteBack = false; +} + + diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp index a1bc1c543..82043606e 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp @@ -1,1160 +1,1160 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuGatheringCollisionTask.h" - -//#define DEBUG_SPU_COLLISION_DETECTION 1 -#include "../SpuDoubleBuffer.h" - -#include "../SpuCollisionTaskProcess.h" -#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS - -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "../SpuContactManifoldCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "SpuContactResult.h" -#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" - -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" - -#include "SpuMinkowskiPenetrationDepthSolver.h" -#include "SpuEpaPenetrationDepthSolver.h" -#include "SpuGjkPairDetector.h" -#include "SpuVoronoiSimplexSolver.h" -#include "boxBoxDistance.h" -#include "BulletMultiThreaded/vectormath2bullet.h" -#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData -#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" - -#ifdef __SPU__ -///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases -#ifndef USE_LIBSPE2 -#define USE_SOFTWARE_CACHE 1 -#endif -#endif //__SPU__ - -//////////////////////////////////////////////// -/// software caching -#if USE_SOFTWARE_CACHE -#include -#include -#include -#include -#define SPE_CACHE_NWAY 4 -//#define SPE_CACHE_NSETS 32, 16 -#define SPE_CACHE_NSETS 8 -//#define SPE_CACHELINE_SIZE 512 -#define SPE_CACHELINE_SIZE 128 -#define SPE_CACHE_SET_TAGID(set) 15 -///make sure that spe_cache.h is below those defines! -#include "../Extras/software_cache/cache/include/spe_cache.h" - - -int g_CacheMisses=0; -int g_CacheHits=0; - -#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version -#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1) -#else -#define spe_cache_read(ea) \ -({ \ - int set, idx, line, byte; \ - _spe_cache_nway_lookup_(ea, set, idx); \ - \ - if (btUnlikely(idx < 0)) { \ - ++g_CacheMisses; \ - idx = _spe_cache_miss_(ea, set, -1); \ - spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \ - spu_mfcstat(MFC_TAG_UPDATE_ALL); \ - } \ - else \ - { \ - ++g_CacheHits; \ - } \ - line = _spe_cacheline_num_(set, idx); \ - byte = _spe_cacheline_byte_offset_(ea); \ - (void *) &spe_cache_mem[line + byte]; \ -}) - -#endif - -#endif // USE_SOFTWARE_CACHE - -bool gUseEpa = false; - -#ifdef USE_SN_TUNER -#include -#endif //USE_SN_TUNER - -#if defined (__SPU__) && !defined (USE_LIBSPE2) -#include -#elif defined (USE_LIBSPE2) -#define spu_printf(a) -#else -#define IGNORE_ALIGNMENT 1 -#include -#include -#define spu_printf printf - -#endif - -//int gNumConvexPoints0=0; - -///Make sure no destructors are called on this memory -struct CollisionTask_LocalStoreMemory -{ - ///This CollisionTask_LocalStoreMemory is mainly used for the SPU version, using explicit DMA - ///Other platforms can use other memory programming models. - - ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]); - DoubleBuffer g_workUnitTaskBuffers; - ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]); - ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]); - ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]); - ///we reserve 32bit integer indices, even though they might be 16bit - ATTRIBUTE_ALIGNED16(int spuIndices[16]); - btPersistentManifold gPersistentManifoldBuffer; - CollisionShape_LocalStoreMemory gCollisionShapes[2]; - bvhMeshShape_LocalStoreMemory bvhShapeData; - SpuConvexPolyhedronVertexData convexVertexData[2]; - CompoundShape_LocalStoreMemory compoundShapeData[2]; - - ///The following pointers might either point into this local store memory, or to the original/other memory locations. - ///See SpuFakeDma for implementation of cellDmaSmallGetReadOnly. - btCollisionObject* m_lsColObj0Ptr; - btCollisionObject* m_lsColObj1Ptr; - btBroadphasePair* m_pairsPointer; - btPersistentManifold* m_lsManifoldPtr; - SpuContactManifoldCollisionAlgorithm* m_lsCollisionAlgorithmPtr; - - bool needsDmaPutContactManifoldAlgo; - - btCollisionObject* getColObj0() - { - return m_lsColObj0Ptr; - } - btCollisionObject* getColObj1() - { - return m_lsColObj1Ptr; - } - - - btBroadphasePair* getBroadphasePairPtr() - { - return m_pairsPointer; - } - - SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm() - { - return m_lsCollisionAlgorithmPtr; - } - - btPersistentManifold* getContactManifoldPtr() - { - return m_lsManifoldPtr; - } -}; - - -#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) - -ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory); - -void* createCollisionLocalStoreMemory() -{ - return &gLocalStoreMemory; -} -#else -void* createCollisionLocalStoreMemory() -{ - return new CollisionTask_LocalStoreMemory; -} - -#endif - -void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts); - - -SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size) -{ -#if USE_SOFTWARE_CACHE - // Check for alignment requirements. We need to make sure the entire request fits within one cache line, - // so the first and last bytes should fall on the same cache line - btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); - - void* ls = spe_cache_read(ea); - memcpy(buffer, ls, size); -#else - stallingUnalignedDmaSmallGet(buffer,ea,size); -#endif -} - -SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0, - void* ls1, ppu_address_t ea1, - void* ls2, ppu_address_t ea2, - size_t size) -{ - btAssert(size<16); - ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]); - ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]); - ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]); - - uint32_t i; - - - ///make sure last 4 bits are the same, for cellDmaSmallGet - char* localStore0 = (char*)ls0; - uint32_t last4BitsOffset = ea0 & 0x0f; - char* tmpTarget0 = tmpBuffer0 + last4BitsOffset; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0); -#else - tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0); -#endif - - - char* localStore1 = (char*)ls1; - last4BitsOffset = ea1 & 0x0f; - char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0); -#else - tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0); -#endif - - char* localStore2 = (char*)ls2; - last4BitsOffset = ea2 & 0x0f; - char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; -#ifdef __SPU__ - cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0); -#else - tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0); -#endif - - - cellDmaWaitTagStatusAll( DMA_MASK(1) ); - - //this is slowish, perhaps memcpy on SPU is smarter? - for (i=0; btLikely( ibvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) - { - unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]); - - small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], - &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], - &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(unsigned short int)); - - m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); - m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); - m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); - } else - { - unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - - small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], - &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], - &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(int)); - } - - // spu_printf("SPU index0=%d ,",spuIndices[0]); - // spu_printf("SPU index1=%d ,",spuIndices[1]); - // spu_printf("SPU index2=%d ,",spuIndices[2]); - // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); - - const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); - for (int j=2;btLikely( j>=0 );j--) - { - int graphicsindex = m_lsMemPtr->spuIndices[j]; - - // spu_printf("SPU index=%d ,",graphicsindex); - btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); - // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); - - - ///handle un-aligned vertices... - - //another DMA for each vertex - small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], - &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], - &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], - sizeof(btScalar)); - - spuTriangleVertices[j] = btVector3( - spuUnscaledVertex[0]*meshScaling.getX(), - spuUnscaledVertex[1]*meshScaling.getY(), - spuUnscaledVertex[2]*meshScaling.getZ()); - - // spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); - } - - - - //btTriangleShape tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]); - - - SpuCollisionPairInput triangleConcaveInput(*m_wuInput); - triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; - triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE; - - m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex); - - // m_spuContacts.flush(); - - ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); - ///this flush should be automatic - // m_spuContacts.flush(); - } - -}; - - -//////////////////////// -/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc) -/////////////////// -void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) -{ - //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite - - btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1]; - //need the mesh interface, for access to triangle vertices - dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape); - - btVector3 aabbMin(-1,-400,-1); - btVector3 aabbMax(1,400,1); - - - //recalc aabbs - btTransform convexInTriangleSpace; - convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0; - btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0]; - - computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace); - - - //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); - //convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - - // btScalar extraMargin = collisionMarginTriangle; - // btVector3 extra(extraMargin,extraMargin,extraMargin); - // aabbMax += extra; - // aabbMin -= extra; - - ///quantize query AABB - unsigned short int quantizedQueryAabbMin[3]; - unsigned short int quantizedQueryAabbMax[3]; - lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0); - lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1); - - QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray(); - //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); - - BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray(); - - - spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts); - IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray(); - //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); - - // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); - //not likely to happen - if (subTrees.size() && indexArray.size() == 1) - { - ///DMA in the index info - dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - //display the headers - int numBatch = subTrees.size(); - for (int i=0;ibvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - // spu_printf("nextBatch = %d\n",nextBatch); - - for (int j=0;jbvhShapeData.gSubtreeHeaders[j]; - - unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap) - { - btAssert(subtree.m_subtreeSize); - - //dma the actual nodes of this subtree - dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - - /* Walk this subtree */ - spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, - &lsMemPtr->bvhShapeData.gSubtreeNodes[0], - 0, - subtree.m_subtreeSize); - } - // spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); - } - - // unsigned short int m_quantizedAabbMin[3]; - // unsigned short int m_quantizedAabbMax[3]; - // int m_rootNodeIndex; - // int m_subtreeSize; - i+=nextBatch; - } - - //pre-fetch first tree, then loop and double buffer - } - -} - - - -//////////////////////// -/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc) -/////////////////// -void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - -#ifdef DEBUG_SPU_COLLISION_DETECTION - //spu_printf("SPU: ProcessSpuConvexConvexCollision\n"); -#endif //DEBUG_SPU_COLLISION_DETECTION - //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0]; - //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1]; - btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; - - bool genericGjk = true; - - if (genericGjk) - { - //try generic GJK - - SpuVoronoiSimplexSolver vsSolver; - SpuMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver; - SpuConvexPenetrationDepthSolver* penetrationSolver; -#ifdef ENABLE_EPA - SpuEpaPenetrationDepthSolver epaPenetrationSolver; - if (gUseEpa) - { - penetrationSolver = &epaPenetrationSolver; - } else -#endif - { - penetrationSolver = &minkowskiPenetrationSolver; - } - - ///DMA in the vertices for convex shapes - ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); - ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]); - - if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - // spu_printf("SPU: DMA btConvexHullShape\n"); - - dmaSize = sizeof(btConvexHullShape); - dmaPpuAddress2 = wuInput->m_collisionShapes[0]; - - cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - // spu_printf("SPU: DMA btConvexHullShape\n"); - dmaSize = sizeof(btConvexHullShape); - dmaPpuAddress2 = wuInput->m_collisionShapes[1]; - cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - //cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - - if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0); - lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0]; - } - - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1); - lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1]; - } - - if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0]; - } - - if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) - { - cellDmaWaitTagStatusAll(DMA_MASK(2)); - lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0]; - } - - - void* shape0Ptr = wuInput->m_spuCollisionShapes[0]; - void* shape1Ptr = wuInput->m_spuCollisionShapes[1]; - int shapeType0 = wuInput->m_shapeType0; - int shapeType1 = wuInput->m_shapeType1; - float marginA = wuInput->m_collisionMargin0; - float marginB = wuInput->m_collisionMargin1; - - SpuClosestPointInput cpInput; - cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0]; - cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1]; - cpInput.m_transformA = wuInput->m_worldTransform0; - cpInput.m_transformB = wuInput->m_worldTransform1; - float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold()); - cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; - - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr(); - //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(), - lsMemPtr->getColObj1()->getWorldTransform(), - lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(), - lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(), - wuInput->m_isSwapped); - - { - SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,penetrationSolver); - gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw); -#ifdef USE_SEPDISTANCE_UTIL - btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold(); - lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1); - lsMemPtr->needsDmaPutContactManifoldAlgo = true; -#endif //USE_SEPDISTANCE_UTIL - - } - - } - - -} - - -template void DoSwap(T& a, T& b) -{ - char tmp[sizeof(T)]; - memcpy(tmp, &a, sizeof(T)); - memcpy(&a, &b, sizeof(T)); - memcpy(&b, tmp, sizeof(T)); -} - -SIMD_FORCE_INLINE void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - - dmaSize = sizeof(btCollisionObject);//btTransform); - dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0(); - lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - dmaSize = sizeof(btCollisionObject);//btTransform); - dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1(); - lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - collisionPairInput.m_worldTransform0 = lsMem.getColObj0()->getWorldTransform(); - collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->getWorldTransform(); -} - - - -void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, - SpuContactResult &spuContacts, - ppu_address_t collisionShape0Ptr, void* collisionShape0Loc, - ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) -{ - - if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) - && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) - { - if (dmaShapes) - { - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - } - - btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; - btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc; - - btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); - btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions(); - - collisionPairInput.m_primitiveDimensions0 = dim0; - collisionPairInput.m_primitiveDimensions1 = dim1; - collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; - collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; - collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; - collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1; - ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts); - } - else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && - btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1)) - { - //snPause(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - // Both are compounds, do N^2 CD for now - ///@todo: add some AABB-based pruning (probably not -> slower) - - btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc; - btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc; - - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); - dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - - dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount0 = spuCompoundShape0->getNumChildShapes(); - int childShapeCount1 = spuCompoundShape1->getNumChildShapes(); - - // Start the N^2 - for (int i = 0; i < childShapeCount0; ++i) - { - btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i]; - - for (int j = 0; j < childShapeCount1; ++j) - { - btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j]; - - /* Create a new collision pair input struct using the two child shapes */ - SpuCollisionPairInput cinput (collisionPairInput); - - cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform; - cinput.m_shapeType0 = childShape0.m_childShapeType; - cinput.m_collisionMargin0 = childShape0.m_childMargin; - - cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform; - cinput.m_shapeType1 = childShape1.m_childShapeType; - cinput.m_collisionMargin1 = childShape1.m_childMargin; - /* Recursively call handleCollisionPair () with new collision pair input */ - handleCollisionPair(cinput, lsMem, spuContacts, - (ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], - (ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); // bug fix: changed index to j. - } - } - } - else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) ) - { - //snPause(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - - // object 0 compound, object 1 non-compound - btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc; - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount = spuCompoundShape->getNumChildShapes(); - - for (int i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; - - // Dma the child shape - dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - SpuCollisionPairInput cinput (collisionPairInput); - cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform; - cinput.m_shapeType0 = childShape.m_childShapeType; - cinput.m_collisionMargin0 = childShape.m_childMargin; - - handleCollisionPair(cinput, lsMem, spuContacts, - (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], - collisionShape1Ptr, collisionShape1Loc, false); - } - } - else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) ) - { - //snPause(); - - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - // object 0 non-compound, object 1 compound - btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc; - dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - int childShapeCount = spuCompoundShape->getNumChildShapes(); - - for (int i = 0; i < childShapeCount; ++i) - { - btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; - // Dma the child shape - dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - SpuCollisionPairInput cinput (collisionPairInput); - cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform; - cinput.m_shapeType1 = childShape.m_childShapeType; - cinput.m_collisionMargin1 = childShape.m_childMargin; - handleCollisionPair(cinput, lsMem, spuContacts, - collisionShape0Ptr, collisionShape0Loc, - (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false); - } - - } - else - { - //a non-convex shape is involved - bool handleConvexConcave = false; - - //snPause(); - - if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) && - btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) - { - // Swap stuff - DoSwap(collisionShape0Ptr, collisionShape1Ptr); - DoSwap(collisionShape0Loc, collisionShape1Loc); - DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1); - DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1); - DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1); - - collisionPairInput.m_isSwapped = true; - } - - if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&& - btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1)) - { - handleConvexConcave = true; - } - if (handleConvexConcave) - { - if (dmaShapes) - { - dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); - dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); - cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); - } - - btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; - btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc; - - btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); - collisionPairInput.m_primitiveDimensions0 = dim0; - collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; - collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; - collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; - collisionPairInput.m_spuCollisionShapes[1] = trimeshShape; - - ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts); - } - - } - - spuContacts.flush(); - -} - - -void processCollisionTask(void* userPtr, void* lsMemPtr) -{ - - SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr; - SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr; - CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr; - CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr); - - gUseEpa = taskDesc.m_useEpa; - - // spu_printf("taskDescPtr=%llx\n",taskDescPtr); - - SpuContactResult spuContacts; - - //////////////////// - - ppu_address_t dmaInPtr = taskDesc.inPtr; - unsigned int numPages = taskDesc.numPages; - unsigned int numOnLastPage = taskDesc.numOnLastPage; - - // prefetch first set of inputs and wait - lsMem.g_workUnitTaskBuffers.init(); - - unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage; - lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); - dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; - - - register unsigned char *inputPtr; - register unsigned int numOnPage; - register unsigned int j; - SpuGatherAndProcessWorkUnitInput* wuInputs; - register int dmaSize; - register ppu_address_t dmaPpuAddress; - register ppu_address_t dmaPpuAddress2; - - int numPairs; - register int p; - SpuCollisionPairInput collisionPairInput; - - for (unsigned int i = 0; btLikely(i < numPages); i++) - { - - // wait for back buffer dma and swap buffers - inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers(); - - // number on current page is number prefetched last iteration - numOnPage = nextNumOnPage; - - - // prefetch next set of inputs -#if MIDPHASE_NUM_WORKUNIT_PAGES > 2 - if ( btLikely( i < numPages-1 ) ) -#else - if ( btUnlikely( i < numPages-1 ) ) -#endif - { - nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE; - lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); - dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; - } - - wuInputs = reinterpret_cast(inputPtr); - - - for (j = 0; btLikely( j < numOnPage ); j++) - { -#ifdef DEBUG_SPU_COLLISION_DETECTION - // printMidphaseInput(&wuInputs[j]); -#endif //DEBUG_SPU_COLLISION_DETECTION - - - numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex; - - if ( btLikely( numPairs ) ) - { - dmaSize = numPairs*sizeof(btBroadphasePair); - dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); - lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - for (p=0;pm_userInfo = %d\n",pair.m_userInfo); - spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm); - spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0); - spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1); -#endif //DEBUG_SPU_COLLISION_DETECTION - - if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1) - { - dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); - dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; - lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - lsMem.needsDmaPutContactManifoldAlgo = false; - - collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr(); - collisionPairInput.m_isSwapped = false; - - if (1) - { - - ///can wait on the combined DMA_MASK, or dma on the same tag - - -#ifdef DEBUG_SPU_COLLISION_DETECTION - // spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0); - // spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1); -#endif //DEBUG_SPU_COLLISION_DETECTION - - - dmaSize = sizeof(btPersistentManifold); - - dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr; - lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - - collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0(); - collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1(); - collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - - - - //??cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - if (1) - { - //snPause(); - - // Get the collision objects - dmaAndSetupCollisionObjects(collisionPairInput, lsMem); - - if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive()) - { - - lsMem.needsDmaPutContactManifoldAlgo = true; -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); -#endif //USE_SEPDISTANCE_UTIL - - - bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&& - (lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE)); - if (boxbox) - { - //spu_printf("boxbox dist = %f\n",distance); - btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); - btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), - lsMem.getColObj1()->getWorldTransform(), - lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), - lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), - collisionPairInput.m_isSwapped); - - - if (//!gUseEpa && -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f -#else - 1 -#endif - ) - { -//#define USE_PE_BOX_BOX 1 -#ifdef USE_PE_BOX_BOX - { - - //getCollisionMargin0 - btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); - btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); - - Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ()); - Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()); - Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()); - Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()); - Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()); - - Transform3 transformA(vmMatrix0,vmPos0); - Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ()); - Transform3 transformB(vmMatrix1,vmPos1); - BoxPoint resultClosestBoxPointA; - BoxPoint resultClosestBoxPointB; - Vector3 resultNormal; - float distanceThreshold = FLT_MAX;//0.0f;//FLT_MAX;//use epsilon? - - - float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold); - - btVector3 normalInB = -getBtVector3(resultNormal); - - if(distance < spuManifold->getContactBreakingThreshold()) - { - btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint)); - - spuContacts.addContactPoint( - normalInB, - pointOnB, - distance); - } - } -#else - { - - btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); - btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); - btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); - btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); - - - btBoxShape box0(shapeDim0); - btBoxShape box1(shapeDim1); - - struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result - { - SpuContactResult& m_spuContacts; - - virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) - { - m_spuContacts.setShapeIdentifiers(partId0,index0,partId1,index1); - } - virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) - { - m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth); - } - - SpuBridgeContactCollector(SpuContactResult& spuContacts) - :m_spuContacts(spuContacts) - { - - } - }; - - SpuBridgeContactCollector bridgeOutput(spuContacts); - - btDiscreteCollisionDetectorInterface::ClosestPointInput input; - input.m_maximumDistanceSquared = 1e30f; - input.m_transformA = collisionPairInput.m_worldTransform0; - input.m_transformB = collisionPairInput.m_worldTransform1; - - btBoxBoxDetector detector(&box0,&box1); - - detector.getClosestPoints(input,bridgeOutput,0); - - } -#endif //USE_PE_BOX_BOX - - lsMem.needsDmaPutContactManifoldAlgo = true; -#ifdef USE_SEPDISTANCE_UTIL - btScalar sepDist = distance+spuManifold->getContactBreakingThreshold(); - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); -#endif //USE_SEPDISTANCE_UTIL - } - - spuContacts.flush(); - - - } else - { - if ( -#ifdef USE_SEPDISTANCE_UTIL - lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f -#else - 1 -#endif //USE_SEPDISTANCE_UTIL - ) - { - handleCollisionPair(collisionPairInput, lsMem, spuContacts, - (ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape, - (ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape); - } else - { - //spu_printf("boxbox dist = %f\n",distance); - btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); - btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; - ppu_address_t manifoldAddress = (ppu_address_t)manifold; - - spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), - lsMem.getColObj1()->getWorldTransform(), - lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), - lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), - collisionPairInput.m_isSwapped); - - spuContacts.flush(); - } - } - - } - - } - } - -#ifdef USE_SEPDISTANCE_UTIL - if (lsMem.needsDmaPutContactManifoldAlgo) - { - dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); - dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; - cellDmaLargePut(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } -#endif //#ifdef USE_SEPDISTANCE_UTIL - - } - } - } - } //end for (j = 0; j < numOnPage; j++) - - }// for - - - - return; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuGatheringCollisionTask.h" + +//#define DEBUG_SPU_COLLISION_DETECTION 1 +#include "../SpuDoubleBuffer.h" + +#include "../SpuCollisionTaskProcess.h" +#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "../SpuContactManifoldCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "SpuContactResult.h" +#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" + +#include "SpuMinkowskiPenetrationDepthSolver.h" +#include "SpuEpaPenetrationDepthSolver.h" +#include "SpuGjkPairDetector.h" +#include "SpuVoronoiSimplexSolver.h" +#include "boxBoxDistance.h" +#include "BulletMultiThreaded/vectormath2bullet.h" +#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData +#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" + +#ifdef __SPU__ +///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases +#ifndef USE_LIBSPE2 +#define USE_SOFTWARE_CACHE 1 +#endif +#endif //__SPU__ + +//////////////////////////////////////////////// +/// software caching +#if USE_SOFTWARE_CACHE +#include +#include +#include +#include +#define SPE_CACHE_NWAY 4 +//#define SPE_CACHE_NSETS 32, 16 +#define SPE_CACHE_NSETS 8 +//#define SPE_CACHELINE_SIZE 512 +#define SPE_CACHELINE_SIZE 128 +#define SPE_CACHE_SET_TAGID(set) 15 +///make sure that spe_cache.h is below those defines! +#include "../Extras/software_cache/cache/include/spe_cache.h" + + +int g_CacheMisses=0; +int g_CacheHits=0; + +#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version +#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1) +#else +#define spe_cache_read(ea) \ +({ \ + int set, idx, line, byte; \ + _spe_cache_nway_lookup_(ea, set, idx); \ + \ + if (btUnlikely(idx < 0)) { \ + ++g_CacheMisses; \ + idx = _spe_cache_miss_(ea, set, -1); \ + spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \ + spu_mfcstat(MFC_TAG_UPDATE_ALL); \ + } \ + else \ + { \ + ++g_CacheHits; \ + } \ + line = _spe_cacheline_num_(set, idx); \ + byte = _spe_cacheline_byte_offset_(ea); \ + (void *) &spe_cache_mem[line + byte]; \ +}) + +#endif + +#endif // USE_SOFTWARE_CACHE + +bool gUseEpa = false; + +#ifdef USE_SN_TUNER +#include +#endif //USE_SN_TUNER + +#if defined (__SPU__) && !defined (USE_LIBSPE2) +#include +#elif defined (USE_LIBSPE2) +#define spu_printf(a) +#else +#define IGNORE_ALIGNMENT 1 +#include +#include +#define spu_printf printf + +#endif + +//int gNumConvexPoints0=0; + +///Make sure no destructors are called on this memory +struct CollisionTask_LocalStoreMemory +{ + ///This CollisionTask_LocalStoreMemory is mainly used for the SPU version, using explicit DMA + ///Other platforms can use other memory programming models. + + ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]); + DoubleBuffer g_workUnitTaskBuffers; + ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]); + ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]); + ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]); + ///we reserve 32bit integer indices, even though they might be 16bit + ATTRIBUTE_ALIGNED16(int spuIndices[16]); + btPersistentManifold gPersistentManifoldBuffer; + CollisionShape_LocalStoreMemory gCollisionShapes[2]; + bvhMeshShape_LocalStoreMemory bvhShapeData; + SpuConvexPolyhedronVertexData convexVertexData[2]; + CompoundShape_LocalStoreMemory compoundShapeData[2]; + + ///The following pointers might either point into this local store memory, or to the original/other memory locations. + ///See SpuFakeDma for implementation of cellDmaSmallGetReadOnly. + btCollisionObject* m_lsColObj0Ptr; + btCollisionObject* m_lsColObj1Ptr; + btBroadphasePair* m_pairsPointer; + btPersistentManifold* m_lsManifoldPtr; + SpuContactManifoldCollisionAlgorithm* m_lsCollisionAlgorithmPtr; + + bool needsDmaPutContactManifoldAlgo; + + btCollisionObject* getColObj0() + { + return m_lsColObj0Ptr; + } + btCollisionObject* getColObj1() + { + return m_lsColObj1Ptr; + } + + + btBroadphasePair* getBroadphasePairPtr() + { + return m_pairsPointer; + } + + SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm() + { + return m_lsCollisionAlgorithmPtr; + } + + btPersistentManifold* getContactManifoldPtr() + { + return m_lsManifoldPtr; + } +}; + + +#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2) + +ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory); + +void* createCollisionLocalStoreMemory() +{ + return &gLocalStoreMemory; +} +#else +void* createCollisionLocalStoreMemory() +{ + return new CollisionTask_LocalStoreMemory; +} + +#endif + +void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts); + + +SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size) +{ +#if USE_SOFTWARE_CACHE + // Check for alignment requirements. We need to make sure the entire request fits within one cache line, + // so the first and last bytes should fall on the same cache line + btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); + + void* ls = spe_cache_read(ea); + memcpy(buffer, ls, size); +#else + stallingUnalignedDmaSmallGet(buffer,ea,size); +#endif +} + +SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0, + void* ls1, ppu_address_t ea1, + void* ls2, ppu_address_t ea2, + size_t size) +{ + btAssert(size<16); + ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]); + + uint32_t i; + + + ///make sure last 4 bits are the same, for cellDmaSmallGet + char* localStore0 = (char*)ls0; + uint32_t last4BitsOffset = ea0 & 0x0f; + char* tmpTarget0 = tmpBuffer0 + last4BitsOffset; +#ifdef __SPU__ + cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0); +#else + tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0); +#endif + + + char* localStore1 = (char*)ls1; + last4BitsOffset = ea1 & 0x0f; + char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; +#ifdef __SPU__ + cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0); +#else + tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0); +#endif + + char* localStore2 = (char*)ls2; + last4BitsOffset = ea2 & 0x0f; + char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; +#ifdef __SPU__ + cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0); +#else + tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0); +#endif + + + cellDmaWaitTagStatusAll( DMA_MASK(1) ); + + //this is slowish, perhaps memcpy on SPU is smarter? + for (i=0; btLikely( ibvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) + { + unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]); + + small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], + &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], + &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(unsigned short int)); + + m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); + m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); + m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); + } else + { + unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + + small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], + &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], + &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(int)); + } + + // spu_printf("SPU index0=%d ,",spuIndices[0]); + // spu_printf("SPU index1=%d ,",spuIndices[1]); + // spu_printf("SPU index2=%d ,",spuIndices[2]); + // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); + + const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); + for (int j=2;btLikely( j>=0 );j--) + { + int graphicsindex = m_lsMemPtr->spuIndices[j]; + + // spu_printf("SPU index=%d ,",graphicsindex); + btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); + // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); + + + ///handle un-aligned vertices... + + //another DMA for each vertex + small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], + &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], + &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], + sizeof(btScalar)); + + spuTriangleVertices[j] = btVector3( + spuUnscaledVertex[0]*meshScaling.getX(), + spuUnscaledVertex[1]*meshScaling.getY(), + spuUnscaledVertex[2]*meshScaling.getZ()); + + // spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); + } + + + + //btTriangleShape tmpTriangleShape(spuTriangleVertices[0],spuTriangleVertices[1],spuTriangleVertices[2]); + + + SpuCollisionPairInput triangleConcaveInput(*m_wuInput); + triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0]; + triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE; + + m_spuContacts.setShapeIdentifiers(-1,-1,subPart,triangleIndex); + + // m_spuContacts.flush(); + + ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts); + ///this flush should be automatic + // m_spuContacts.flush(); + } + +}; + + +//////////////////////// +/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc) +/////////////////// +void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) +{ + //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite + + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1]; + //need the mesh interface, for access to triangle vertices + dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape); + + btVector3 aabbMin(-1,-400,-1); + btVector3 aabbMax(1,400,1); + + + //recalc aabbs + btTransform convexInTriangleSpace; + convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0; + btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0]; + + computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace); + + + //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); + //convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); + + // btScalar extraMargin = collisionMarginTriangle; + // btVector3 extra(extraMargin,extraMargin,extraMargin); + // aabbMax += extra; + // aabbMin -= extra; + + ///quantize query AABB + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0); + lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1); + + QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray(); + //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); + + BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray(); + + + spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts); + IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray(); + //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); + + // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); + //not likely to happen + if (subTrees.size() && indexArray.size() == 1) + { + ///DMA in the index info + dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + //display the headers + int numBatch = subTrees.size(); + for (int i=0;ibvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + // spu_printf("nextBatch = %d\n",nextBatch); + + for (int j=0;jbvhShapeData.gSubtreeHeaders[j]; + + unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + { + btAssert(subtree.m_subtreeSize); + + //dma the actual nodes of this subtree + dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + /* Walk this subtree */ + spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, + &lsMemPtr->bvhShapeData.gSubtreeNodes[0], + 0, + subtree.m_subtreeSize); + } + // spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); + } + + // unsigned short int m_quantizedAabbMin[3]; + // unsigned short int m_quantizedAabbMax[3]; + // int m_rootNodeIndex; + // int m_subtreeSize; + i+=nextBatch; + } + + //pre-fetch first tree, then loop and double buffer + } + +} + + + +//////////////////////// +/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc) +/////////////////// +void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts) +{ + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + +#ifdef DEBUG_SPU_COLLISION_DETECTION + //spu_printf("SPU: ProcessSpuConvexConvexCollision\n"); +#endif //DEBUG_SPU_COLLISION_DETECTION + //CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0]; + //CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1]; + btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr; + + bool genericGjk = true; + + if (genericGjk) + { + //try generic GJK + + SpuVoronoiSimplexSolver vsSolver; + SpuMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver; + SpuConvexPenetrationDepthSolver* penetrationSolver; +#ifdef ENABLE_EPA + SpuEpaPenetrationDepthSolver epaPenetrationSolver; + if (gUseEpa) + { + penetrationSolver = &epaPenetrationSolver; + } else +#endif + { + penetrationSolver = &minkowskiPenetrationSolver; + } + + ///DMA in the vertices for convex shapes + ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]); + ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]); + + if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + // spu_printf("SPU: DMA btConvexHullShape\n"); + + dmaSize = sizeof(btConvexHullShape); + dmaPpuAddress2 = wuInput->m_collisionShapes[0]; + + cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + // spu_printf("SPU: DMA btConvexHullShape\n"); + dmaSize = sizeof(btConvexHullShape); + dmaPpuAddress2 = wuInput->m_collisionShapes[1]; + cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + //cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + + if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(1)); + dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0); + lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0]; + } + + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(1)); + dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1); + lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1]; + } + + if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(2)); + lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0]; + } + + if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) ) + { + cellDmaWaitTagStatusAll(DMA_MASK(2)); + lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0]; + } + + + void* shape0Ptr = wuInput->m_spuCollisionShapes[0]; + void* shape1Ptr = wuInput->m_spuCollisionShapes[1]; + int shapeType0 = wuInput->m_shapeType0; + int shapeType1 = wuInput->m_shapeType1; + float marginA = wuInput->m_collisionMargin0; + float marginB = wuInput->m_collisionMargin1; + + SpuClosestPointInput cpInput; + cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0]; + cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1]; + cpInput.m_transformA = wuInput->m_worldTransform0; + cpInput.m_transformB = wuInput->m_worldTransform1; + float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold()); + cpInput.m_maximumDistanceSquared = sumMargin * sumMargin; + + ppu_address_t manifoldAddress = (ppu_address_t)manifold; + + btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr(); + //spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped); + spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(), + lsMemPtr->getColObj1()->getWorldTransform(), + lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(), + lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(), + wuInput->m_isSwapped); + + { + SpuGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&vsSolver,penetrationSolver); + gjk.getClosestPoints(cpInput,spuContacts);//,debugDraw); +#ifdef USE_SEPDISTANCE_UTIL + btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold(); + lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1); + lsMemPtr->needsDmaPutContactManifoldAlgo = true; +#endif //USE_SEPDISTANCE_UTIL + + } + + } + + +} + + +template void DoSwap(T& a, T& b) +{ + char tmp[sizeof(T)]; + memcpy(tmp, &a, sizeof(T)); + memcpy(&a, &b, sizeof(T)); + memcpy(&b, tmp, sizeof(T)); +} + +SIMD_FORCE_INLINE void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem) +{ + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + + dmaSize = sizeof(btCollisionObject);//btTransform); + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0(); + lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + + dmaSize = sizeof(btCollisionObject);//btTransform); + dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1(); + lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + collisionPairInput.m_worldTransform0 = lsMem.getColObj0()->getWorldTransform(); + collisionPairInput.m_worldTransform1 = lsMem.getColObj1()->getWorldTransform(); +} + + + +void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem, + SpuContactResult &spuContacts, + ppu_address_t collisionShape0Ptr, void* collisionShape0Loc, + ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true) +{ + + if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0) + && btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) + { + if (dmaShapes) + { + dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); + dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + } + + btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; + btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc; + + btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); + btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions(); + + collisionPairInput.m_primitiveDimensions0 = dim0; + collisionPairInput.m_primitiveDimensions1 = dim1; + collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; + collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; + collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; + collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1; + ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts); + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) && + btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1)) + { + //snPause(); + + dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); + dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + // Both are compounds, do N^2 CD for now + ///@todo: add some AABB-based pruning (probably not -> slower) + + btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc; + btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc; + + dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); + dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + + dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + int childShapeCount0 = spuCompoundShape0->getNumChildShapes(); + int childShapeCount1 = spuCompoundShape1->getNumChildShapes(); + + // Start the N^2 + for (int i = 0; i < childShapeCount0; ++i) + { + btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i]; + + for (int j = 0; j < childShapeCount1; ++j) + { + btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j]; + + /* Create a new collision pair input struct using the two child shapes */ + SpuCollisionPairInput cinput (collisionPairInput); + + cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform; + cinput.m_shapeType0 = childShape0.m_childShapeType; + cinput.m_collisionMargin0 = childShape0.m_childMargin; + + cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform; + cinput.m_shapeType1 = childShape1.m_childShapeType; + cinput.m_collisionMargin1 = childShape1.m_childMargin; + /* Recursively call handleCollisionPair () with new collision pair input */ + handleCollisionPair(cinput, lsMem, spuContacts, + (ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], + (ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false); // bug fix: changed index to j. + } + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) ) + { + //snPause(); + + dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); + dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + + // object 0 compound, object 1 non-compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc; + dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + for (int i = 0; i < childShapeCount; ++i) + { + btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; + + // Dma the child shape + dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + SpuCollisionPairInput cinput (collisionPairInput); + cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform; + cinput.m_shapeType0 = childShape.m_childShapeType; + cinput.m_collisionMargin0 = childShape.m_childMargin; + + handleCollisionPair(cinput, lsMem, spuContacts, + (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], + collisionShape1Ptr, collisionShape1Loc, false); + } + } + else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) ) + { + //snPause(); + + dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); + dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + // object 0 non-compound, object 1 compound + btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc; + dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + int childShapeCount = spuCompoundShape->getNumChildShapes(); + + for (int i = 0; i < childShapeCount; ++i) + { + btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i]; + // Dma the child shape + dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + SpuCollisionPairInput cinput (collisionPairInput); + cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform; + cinput.m_shapeType1 = childShape.m_childShapeType; + cinput.m_collisionMargin1 = childShape.m_childMargin; + handleCollisionPair(cinput, lsMem, spuContacts, + collisionShape0Ptr, collisionShape0Loc, + (ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false); + } + + } + else + { + //a non-convex shape is involved + bool handleConvexConcave = false; + + //snPause(); + + if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) && + btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1)) + { + // Swap stuff + DoSwap(collisionShape0Ptr, collisionShape1Ptr); + DoSwap(collisionShape0Loc, collisionShape1Loc); + DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1); + DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1); + DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1); + + collisionPairInput.m_isSwapped = true; + } + + if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&& + btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1)) + { + handleConvexConcave = true; + } + if (handleConvexConcave) + { + if (dmaShapes) + { + dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0); + dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1); + cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2)); + } + + btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc; + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc; + + btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions(); + collisionPairInput.m_primitiveDimensions0 = dim0; + collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr; + collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr; + collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0; + collisionPairInput.m_spuCollisionShapes[1] = trimeshShape; + + ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts); + } + + } + + spuContacts.flush(); + +} + + +void processCollisionTask(void* userPtr, void* lsMemPtr) +{ + + SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr; + SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr; + CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr; + CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr); + + gUseEpa = taskDesc.m_useEpa; + + // spu_printf("taskDescPtr=%llx\n",taskDescPtr); + + SpuContactResult spuContacts; + + //////////////////// + + ppu_address_t dmaInPtr = taskDesc.inPtr; + unsigned int numPages = taskDesc.numPages; + unsigned int numOnLastPage = taskDesc.numOnLastPage; + + // prefetch first set of inputs and wait + lsMem.g_workUnitTaskBuffers.init(); + + unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage; + lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); + dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; + + + register unsigned char *inputPtr; + register unsigned int numOnPage; + register unsigned int j; + SpuGatherAndProcessWorkUnitInput* wuInputs; + register int dmaSize; + register ppu_address_t dmaPpuAddress; + register ppu_address_t dmaPpuAddress2; + + int numPairs; + register int p; + SpuCollisionPairInput collisionPairInput; + + for (unsigned int i = 0; btLikely(i < numPages); i++) + { + + // wait for back buffer dma and swap buffers + inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers(); + + // number on current page is number prefetched last iteration + numOnPage = nextNumOnPage; + + + // prefetch next set of inputs +#if MIDPHASE_NUM_WORKUNIT_PAGES > 2 + if ( btLikely( i < numPages-1 ) ) +#else + if ( btUnlikely( i < numPages-1 ) ) +#endif + { + nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE; + lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3)); + dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE; + } + + wuInputs = reinterpret_cast(inputPtr); + + + for (j = 0; btLikely( j < numOnPage ); j++) + { +#ifdef DEBUG_SPU_COLLISION_DETECTION + // printMidphaseInput(&wuInputs[j]); +#endif //DEBUG_SPU_COLLISION_DETECTION + + + numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex; + + if ( btLikely( numPairs ) ) + { + dmaSize = numPairs*sizeof(btBroadphasePair); + dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair); + lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + for (p=0;pm_userInfo = %d\n",pair.m_userInfo); + spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm); + spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0); + spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1); +#endif //DEBUG_SPU_COLLISION_DETECTION + + if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1) + { + dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); + dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; + lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + lsMem.needsDmaPutContactManifoldAlgo = false; + + collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr(); + collisionPairInput.m_isSwapped = false; + + if (1) + { + + ///can wait on the combined DMA_MASK, or dma on the same tag + + +#ifdef DEBUG_SPU_COLLISION_DETECTION + // spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0); + // spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1); +#endif //DEBUG_SPU_COLLISION_DETECTION + + + dmaSize = sizeof(btPersistentManifold); + + dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr; + lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + + collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0(); + collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1(); + collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); + collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); + + + + //??cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + if (1) + { + //snPause(); + + // Get the collision objects + dmaAndSetupCollisionObjects(collisionPairInput, lsMem); + + if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive()) + { + + lsMem.needsDmaPutContactManifoldAlgo = true; +#ifdef USE_SEPDISTANCE_UTIL + lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); +#endif //USE_SEPDISTANCE_UTIL + + + bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&& + (lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE)); + if (boxbox) + { + //spu_printf("boxbox dist = %f\n",distance); + btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); + btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; + ppu_address_t manifoldAddress = (ppu_address_t)manifold; + + spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), + lsMem.getColObj1()->getWorldTransform(), + lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), + lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), + collisionPairInput.m_isSwapped); + + + if (//!gUseEpa && +#ifdef USE_SEPDISTANCE_UTIL + lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f +#else + 1 +#endif + ) + { +//#define USE_PE_BOX_BOX 1 +#ifdef USE_PE_BOX_BOX + { + + //getCollisionMargin0 + btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); + btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); + btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); + btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); + + Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ()); + Vector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()); + Vector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()); + Matrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()); + Matrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()); + + Transform3 transformA(vmMatrix0,vmPos0); + Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ()); + Transform3 transformB(vmMatrix1,vmPos1); + BoxPoint resultClosestBoxPointA; + BoxPoint resultClosestBoxPointB; + Vector3 resultNormal; + float distanceThreshold = FLT_MAX;//0.0f;//FLT_MAX;//use epsilon? + + + float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold); + + btVector3 normalInB = -getBtVector3(resultNormal); + + if(distance < spuManifold->getContactBreakingThreshold()) + { + btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(resultClosestBoxPointB.localPoint)); + + spuContacts.addContactPoint( + normalInB, + pointOnB, + distance); + } + } +#else + { + + btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0(); + btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1(); + btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0); + btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1); + + + btBoxShape box0(shapeDim0); + btBoxShape box1(shapeDim1); + + struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result + { + SpuContactResult& m_spuContacts; + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + m_spuContacts.setShapeIdentifiers(partId0,index0,partId1,index1); + } + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth); + } + + SpuBridgeContactCollector(SpuContactResult& spuContacts) + :m_spuContacts(spuContacts) + { + + } + }; + + SpuBridgeContactCollector bridgeOutput(spuContacts); + + btDiscreteCollisionDetectorInterface::ClosestPointInput input; + input.m_maximumDistanceSquared = 1e30f; + input.m_transformA = collisionPairInput.m_worldTransform0; + input.m_transformB = collisionPairInput.m_worldTransform1; + + btBoxBoxDetector detector(&box0,&box1); + + detector.getClosestPoints(input,bridgeOutput,0); + + } +#endif //USE_PE_BOX_BOX + + lsMem.needsDmaPutContactManifoldAlgo = true; +#ifdef USE_SEPDISTANCE_UTIL + btScalar sepDist = distance+spuManifold->getContactBreakingThreshold(); + lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1); +#endif //USE_SEPDISTANCE_UTIL + } + + spuContacts.flush(); + + + } else + { + if ( +#ifdef USE_SEPDISTANCE_UTIL + lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f +#else + 1 +#endif //USE_SEPDISTANCE_UTIL + ) + { + handleCollisionPair(collisionPairInput, lsMem, spuContacts, + (ppu_address_t)lsMem.getColObj0()->getCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape, + (ppu_address_t)lsMem.getColObj1()->getCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape); + } else + { + //spu_printf("boxbox dist = %f\n",distance); + btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr(); + btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr; + ppu_address_t manifoldAddress = (ppu_address_t)manifold; + + spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(), + lsMem.getColObj1()->getWorldTransform(), + lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(), + lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(), + collisionPairInput.m_isSwapped); + + spuContacts.flush(); + } + } + + } + + } + } + +#ifdef USE_SEPDISTANCE_UTIL + if (lsMem.needsDmaPutContactManifoldAlgo) + { + dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm); + dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm; + cellDmaLargePut(&lsMem.gSpuContactManifoldAlgo, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } +#endif //#ifdef USE_SEPDISTANCE_UTIL + + } + } + } + } //end for (j = 0; j < numOnPage; j++) + + }// for + + + + return; +} diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp index b06757692..be4bf5a54 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp @@ -1,851 +1,851 @@ -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "SpuCollisionShapes.h" -#include "SpuGjkEpa2.h" - -#if defined(DEBUG) || defined (_DEBUG) -#include //for debug printf -#ifdef __SPU__ -#include -#define printf spu_printf -#endif //__SPU__ -#endif - -namespace gjkepa2_spu_impl -{ - -// Config - - /* GJK */ -#define GJK_MAX_ITERATIONS 128 -#define GJK_ACCURARY ((btScalar)0.0001) -#define GJK_MIN_DISTANCE ((btScalar)0.0001) -#define GJK_DUPLICATED_EPS ((btScalar)0.0001) -#define GJK_SIMPLEX2_EPS ((btScalar)0.0) -#define GJK_SIMPLEX3_EPS ((btScalar)0.0) -#define GJK_SIMPLEX4_EPS ((btScalar)0.0) - - /* EPA */ -#define EPA_MAX_VERTICES 64 -#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) -#define EPA_MAX_ITERATIONS 255 -#define EPA_ACCURACY ((btScalar)0.0001) -#define EPA_FALLBACK (10*EPA_ACCURACY) -#define EPA_PLANE_EPS ((btScalar)0.00001) -#define EPA_INSIDE_EPS ((btScalar)0.01) - - -// Shorthands -typedef unsigned int U; -typedef unsigned char U1; - -struct convexShape -{ - void* shape; - SpuConvexPolyhedronVertexData* convexData; - int shapeType; - float margin; -}; - -// MinkowskiDiff -struct MinkowskiDiff - { - convexShape m_shapes[2]; - btMatrix3x3 m_toshape1; - btTransform m_toshape0; - btVector3 (btConvexShape::*Ls)(const btVector3&) const; - void EnableMargin(bool enable) - { -#if 0 - if(enable) - Ls=&btConvexShape::localGetSupportingVertex; - else - Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; -#endif - } - inline btVector3 Support0(const btVector3& d) const - { - btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData); - btVector3 ud = d; - ud.normalize(); - sp += ud * m_shapes[0].margin; - return sp; - // return(((m_shapes[0])->*(Ls))(d)); - } - inline btVector3 Support1(const btVector3& d) const - { - btVector3 nd = m_toshape1*d; - btVector3 ud = nd; - ud.normalize (); - btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData); - sp += ud * m_shapes[1].margin; - return m_toshape0 * sp; - // return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); - } - inline btVector3 Support(const btVector3& d) const - { - return(Support0(d)-Support1(-d)); - } - btVector3 Support(const btVector3& d,U index) const - { - - if(index) - return(Support1(d)); - else - return(Support0(d)); - - } - }; - -typedef MinkowskiDiff tShape; - -// GJK -struct GJK -{ -/* Types */ -struct sSV - { - btVector3 d,w; - }; -struct sSimplex - { - sSV* c[4]; - btScalar p[4]; - U rank; - }; -struct eStatus { enum _ { - Valid, - Inside, - Failed };}; -/* Fields */ -tShape m_shape; -btVector3 m_ray; -btScalar m_distance; -sSimplex m_simplices[2]; -sSV m_store[4]; -sSV* m_free[4]; -U m_nfree; -U m_current; -sSimplex* m_simplex; -eStatus::_ m_status; -/* Methods */ - GJK() - { - Initialize(); - } -void Initialize() - { - m_ray = btVector3(0,0,0); - m_nfree = 0; - m_status = eStatus::Failed; - m_current = 0; - m_distance = 0; - } -eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) - { - U iterations=0; - btScalar sqdist=0; - btScalar alpha=0; - btVector3 lastw[4]; - U clastw=0; - /* Initialize solver */ - m_free[0] = &m_store[0]; - m_free[1] = &m_store[1]; - m_free[2] = &m_store[2]; - m_free[3] = &m_store[3]; - m_nfree = 4; - m_current = 0; - m_status = eStatus::Valid; - m_shape = shapearg; - m_distance = 0; - /* Initialize simplex */ - m_simplices[0].rank = 0; - m_ray = guess; - const btScalar sqrl= m_ray.length2(); - appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); - m_simplices[0].p[0] = 1; - m_ray = m_simplices[0].c[0]->w; - sqdist = sqrl; - lastw[0] = - lastw[1] = - lastw[2] = - lastw[3] = m_ray; - /* Loop */ - do { - const U next=1-m_current; - sSimplex& cs=m_simplices[m_current]; - sSimplex& ns=m_simplices[next]; - /* Check zero */ - const btScalar rl=m_ray.length(); - if(rlw; - bool found=false; - for(U i=0;i<4;++i) - { - if((w-lastw[i]).length2()w, - cs.c[1]->w, - weights,mask);break; - case 3: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - weights,mask);break; - case 4: sqdist=projectorigin( cs.c[0]->w, - cs.c[1]->w, - cs.c[2]->w, - cs.c[3]->w, - weights,mask);break; - } - if(sqdist>=0) - {/* Valid */ - ns.rank = 0; - m_ray = btVector3(0,0,0); - m_current = next; - for(U i=0,ni=cs.rank;iw*weights[i]; - } - else - { - m_free[m_nfree++] = cs.c[i]; - } - } - if(mask==15) m_status=eStatus::Inside; - } - else - {/* Return old simplex */ - removevertice(m_simplices[m_current]); - break; - } - m_status=((++iterations)rank) - { - case 1: - { - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - appendvertice(*m_simplex, axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-axis); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 2: - { - const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; - for(U i=0;i<3;++i) - { - btVector3 axis=btVector3(0,0,0); - axis[i]=1; - if(btFabs(dot(axis,d))>0) - { - const btVector3 p=cross(d,axis); - appendvertice(*m_simplex, p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-p); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - } - break; - case 3: - { - const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, - m_simplex->c[2]->w-m_simplex->c[0]->w); - const btScalar l=n.length(); - if(l>0) - { - appendvertice(*m_simplex,n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - appendvertice(*m_simplex,-n); - if(EncloseOrigin()) return(true); - removevertice(*m_simplex); - } - } - break; - case 4: - { - if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, - m_simplex->c[1]->w-m_simplex->c[3]->w, - m_simplex->c[2]->w-m_simplex->c[3]->w))>0) - return(true); - } - break; - } - return(false); - } -/* Internals */ -void getsupport(const btVector3& d,sSV& sv) const - { - sv.d = d/d.length(); - sv.w = m_shape.Support(sv.d); - } -void removevertice(sSimplex& simplex) - { - m_free[m_nfree++]=simplex.c[--simplex.rank]; - } -void appendvertice(sSimplex& simplex,const btVector3& v) - { - simplex.p[simplex.rank]=0; - simplex.c[simplex.rank]=m_free[--m_nfree]; - getsupport(v,*simplex.c[simplex.rank++]); - } -static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) - { - return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- - a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ - a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - btScalar* w,U& m) - { - const btVector3 d=b-a; - const btScalar l=d.length2(); - if(l>GJK_SIMPLEX2_EPS) - { - const btScalar t(l>0?-dot(a,d)/l:0); - if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } - else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } - else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } - } - return(-1); - } -static btScalar projectorigin( const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar* w,U& m) - { - static const U imd3[]={1,2,0}; - const btVector3* vt[]={&a,&b,&c}; - const btVector3 dl[]={a-b,b-c,c-a}; - const btVector3 n=cross(dl[0],dl[1]); - const btScalar l=n.length2(); - if(l>GJK_SIMPLEX3_EPS) - { - btScalar mindist=-1; - btScalar subw[2] = { btScalar(0.0f), btScalar(0.0f) }; - U subm; - for(U i=0;i<3;++i) - { - if(dot(*vt[i],cross(dl[i],n))>0) - { - const U j=imd3[i]; - const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); - if((mindist<0)||(subdGJK_SIMPLEX4_EPS)) - { - btScalar mindist=-1; - btScalar subw[3]; - U subm; - for(U i=0;i<3;++i) - { - const U j=imd3[i]; - const btScalar s=vl*dot(d,cross(dl[i],dl[j])); - if(s>0) - { - const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); - if((mindist<0)||(subd1)&&gjk.EncloseOrigin()) - { - /* Clean up */ - while(m_hull.root) - { - sFace* f(m_hull.root); - remove(m_hull,f); - append(m_stock,f); - } - m_status = eStatus::Valid; - m_nextsv = 0; - /* Orient simplex */ - if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, - simplex.c[1]->w-simplex.c[3]->w, - simplex.c[2]->w-simplex.c[3]->w)<0) - { - btSwap(simplex.c[0],simplex.c[1]); - btSwap(simplex.p[0],simplex.p[1]); - } - /* Build initial hull */ - sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), - newface(simplex.c[1],simplex.c[0],simplex.c[3],true), - newface(simplex.c[2],simplex.c[1],simplex.c[3],true), - newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; - if(m_hull.count==4) - { - sFace* best=findbest(); - sFace outer=*best; - U pass=0; - U iterations=0; - bind(tetra[0],0,tetra[1],0); - bind(tetra[0],1,tetra[2],0); - bind(tetra[0],2,tetra[3],0); - bind(tetra[1],1,tetra[3],2); - bind(tetra[1],2,tetra[2],1); - bind(tetra[2],2,tetra[3],1); - m_status=eStatus::Valid; - for(;iterationspass = (U1)(++pass); - gjk.getsupport(best->n,*w); - const btScalar wdist=dot(best->n,w->w)-best->d; - if(wdist>EPA_ACCURACY) - { - for(U j=0;(j<3)&&valid;++j) - { - valid&=expand( pass,w, - best->f[j],best->e[j], - horizon); - } - if(valid&&(horizon.nf>=3)) - { - bind(horizon.cf,1,horizon.ff,2); - remove(m_hull,best); - append(m_stock,best); - best=findbest(); - if(best->p>=outer.p) outer=*best; - } else { m_status=eStatus::InvalidHull;break; } - } else { m_status=eStatus::AccuraryReached;break; } - } else { m_status=eStatus::OutOfVertices;break; } - } - const btVector3 projection=outer.n*outer.d; - m_normal = outer.n; - m_depth = outer.d; - m_result.rank = 3; - m_result.c[0] = outer.c[0]; - m_result.c[1] = outer.c[1]; - m_result.c[2] = outer.c[2]; - m_result.p[0] = cross( outer.c[1]->w-projection, - outer.c[2]->w-projection).length(); - m_result.p[1] = cross( outer.c[2]->w-projection, - outer.c[0]->w-projection).length(); - m_result.p[2] = cross( outer.c[0]->w-projection, - outer.c[1]->w-projection).length(); - const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; - m_result.p[0] /= sum; - m_result.p[1] /= sum; - m_result.p[2] /= sum; - return(m_status); - } - } - /* Fallback */ - m_status = eStatus::FallBack; - m_normal = -guess; - const btScalar nl=m_normal.length(); - if(nl>0) - m_normal = m_normal/nl; - else - m_normal = btVector3(1,0,0); - m_depth = 0; - m_result.rank=1; - m_result.c[0]=simplex.c[0]; - m_result.p[0]=1; - return(m_status); - } -sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) - { - if(m_stock.root) - { - sFace* face=m_stock.root; - remove(m_stock,face); - append(m_hull,face); - face->pass = 0; - face->c[0] = a; - face->c[1] = b; - face->c[2] = c; - face->n = cross(b->w-a->w,c->w-a->w); - const btScalar l=face->n.length(); - const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - dot(a->w,cross(face->n,a->w-b->w)), - dot(b->w,cross(face->n,b->w-c->w))), - dot(c->w,cross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; - if(v) - { - face->d = dot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) - { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); - } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); - } -sFace* findbest() - { - sFace* minf=m_hull.root; - btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; - for(sFace* f=minf->l[1];f;f=f->l[1]) - { - const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqdp; - } - } - return(minf); - } -bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) - { - static const U i1m3[]={1,2,0}; - static const U i2m3[]={2,0,1}; - if(f->pass!=pass) - { - const U e1=i1m3[e]; - if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) - { - sFace* nf=newface(f->c[e1],f->c[e],w,false); - if(nf) - { - bind(nf,0,f,e); - if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; - horizon.cf=nf; - ++horizon.nf; - return(true); - } - } - else - { - const U e2=i2m3[e]; - f->pass = (U1)pass; - if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& - expand(pass,w,f->f[e2],f->e[e2],horizon)) - { - remove(m_hull,f); - append(m_stock,f); - return(true); - } - } - } - return(false); - } -static inline void bind(sFace* fa,U ea,sFace* fb,U eb) - { - fa->e[ea]=(U1)eb;fa->f[ea]=fb; - fb->e[eb]=(U1)ea;fb->f[eb]=fa; - } -static inline void append(sList& list,sFace* face) - { - face->l[0] = 0; - face->l[1] = list.root; - if(list.root) list.root->l[0]=face; - list.root = face; - ++list.count; - } -static inline void remove(sList& list,sFace* face) - { - if(face->l[1]) face->l[1]->l[0]=face->l[0]; - if(face->l[0]) face->l[0]->l[1]=face->l[1]; - if(face==list.root) list.root=face->l[1]; - --list.count; - } -}; - -// -static void Initialize(void* shapeA, - SpuConvexPolyhedronVertexData* convexDataA, - int shapeTypeA, - float marginA, - const btTransform& wtrs0, - void* shapeB, - SpuConvexPolyhedronVertexData* convexDataB, - int shapeTypeB, - float marginB, - const btTransform& wtrs1, - SpuGjkEpaSolver2::sResults& results, - tShape& shape, - bool withmargins) -{ -/* Results */ -results.witnesses[0] = -results.witnesses[1] = btVector3(0,0,0); -results.status = SpuGjkEpaSolver2::sResults::Separated; -/* Shape */ -shape.m_shapes[0].margin = marginA; -shape.m_shapes[0].shape = shapeA; -shape.m_shapes[0].shapeType = shapeTypeA; -shape.m_shapes[0].convexData = convexDataA; -shape.m_shapes[1].margin = marginB; -shape.m_shapes[1].shape = shapeB; -shape.m_shapes[1].shapeType = shapeTypeB; -shape.m_shapes[1].convexData = convexDataB; -shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); -shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); -shape.EnableMargin(withmargins); -} - -} - -// -// Api -// - -using namespace gjkepa2_spu_impl; - -// -int SpuGjkEpaSolver2::StackSizeRequirement() -{ -return(sizeof(GJK)+sizeof(EPA)); -} - -// -bool SpuGjkEpaSolver2::Penetration(void* shapeA, - SpuConvexPolyhedronVertexData* convexDataA, - int shapeTypeA, - float marginA, - const btTransform& wtrs0, - void* shapeB, - SpuConvexPolyhedronVertexData* convexDataB, - int shapeTypeB, - float marginB, - const btTransform& wtrs1, - const btVector3& guess, - sResults& results) -{ -tShape shape; -Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true); -GJK gjk; -GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); -switch(gjk_status) - { - case GJK::eStatus::Inside: - { - EPA epa; - EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); - if(epa_status!=EPA::eStatus::Failed) - { - btVector3 w0=btVector3(0,0,0); - for(U i=0;id,0)*epa.m_result.p[i]; - } - results.status = sResults::Penetrating; - results.witnesses[0] = wtrs0*w0; - results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); - return(true); - } else results.status=sResults::EPA_Failed; - } - break; - case GJK::eStatus::Failed: - results.status=sResults::GJK_Failed; - break; - } -return(false); -} - -/* Symbols cleanup */ - -#undef GJK_MAX_ITERATIONS -#undef GJK_ACCURARY -#undef GJK_MIN_DISTANCE -#undef GJK_DUPLICATED_EPS -#undef GJK_SIMPLEX2_EPS -#undef GJK_SIMPLEX3_EPS -#undef GJK_SIMPLEX4_EPS - -#undef EPA_MAX_VERTICES -#undef EPA_MAX_FACES -#undef EPA_MAX_ITERATIONS -#undef EPA_ACCURACY -#undef EPA_FALLBACK -#undef EPA_PLANE_EPS -#undef EPA_INSIDE_EPS +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "SpuCollisionShapes.h" +#include "SpuGjkEpa2.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include //for debug printf +#ifdef __SPU__ +#include +#define printf spu_printf +#endif //__SPU__ +#endif + +namespace gjkepa2_spu_impl +{ + +// Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((btScalar)0.0001) +#define GJK_MIN_DISTANCE ((btScalar)0.0001) +#define GJK_DUPLICATED_EPS ((btScalar)0.0001) +#define GJK_SIMPLEX2_EPS ((btScalar)0.0) +#define GJK_SIMPLEX3_EPS ((btScalar)0.0) +#define GJK_SIMPLEX4_EPS ((btScalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((btScalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((btScalar)0.00001) +#define EPA_INSIDE_EPS ((btScalar)0.01) + + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +struct convexShape +{ + void* shape; + SpuConvexPolyhedronVertexData* convexData; + int shapeType; + float margin; +}; + +// MinkowskiDiff +struct MinkowskiDiff + { + convexShape m_shapes[2]; + btMatrix3x3 m_toshape1; + btTransform m_toshape0; + btVector3 (btConvexShape::*Ls)(const btVector3&) const; + void EnableMargin(bool enable) + { +#if 0 + if(enable) + Ls=&btConvexShape::localGetSupportingVertex; + else + Ls=&btConvexShape::localGetSupportingVertexWithoutMargin; +#endif + } + inline btVector3 Support0(const btVector3& d) const + { + btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[0].shapeType, m_shapes[0].shape, d, m_shapes[0].convexData); + btVector3 ud = d; + ud.normalize(); + sp += ud * m_shapes[0].margin; + return sp; + // return(((m_shapes[0])->*(Ls))(d)); + } + inline btVector3 Support1(const btVector3& d) const + { + btVector3 nd = m_toshape1*d; + btVector3 ud = nd; + ud.normalize (); + btVector3 sp = localGetSupportingVertexWithoutMargin (m_shapes[1].shapeType, m_shapes[1].shape, nd, m_shapes[1].convexData); + sp += ud * m_shapes[1].margin; + return m_toshape0 * sp; + // return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d)); + } + inline btVector3 Support(const btVector3& d) const + { + return(Support0(d)-Support1(-d)); + } + btVector3 Support(const btVector3& d,U index) const + { + + if(index) + return(Support1(d)); + else + return(Support0(d)); + + } + }; + +typedef MinkowskiDiff tShape; + +// GJK +struct GJK +{ +/* Types */ +struct sSV + { + btVector3 d,w; + }; +struct sSimplex + { + sSV* c[4]; + btScalar p[4]; + U rank; + }; +struct eStatus { enum _ { + Valid, + Inside, + Failed };}; +/* Fields */ +tShape m_shape; +btVector3 m_ray; +btScalar m_distance; +sSimplex m_simplices[2]; +sSV m_store[4]; +sSV* m_free[4]; +U m_nfree; +U m_current; +sSimplex* m_simplex; +eStatus::_ m_status; +/* Methods */ + GJK() + { + Initialize(); + } +void Initialize() + { + m_ray = btVector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } +eStatus::_ Evaluate(const tShape& shapearg,const btVector3& guess) + { + U iterations=0; + btScalar sqdist=0; + btScalar alpha=0; + btVector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const btScalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const btScalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = btVector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + btVector3 axis=btVector3(0,0,0); + axis[i]=1; + if(btFabs(dot(axis,d))>0) + { + const btVector3 p=cross(d,axis); + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const btVector3 n=cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + const btScalar l=n.length(); + if(l>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } +/* Internals */ +void getsupport(const btVector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } +void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } +void appendvertice(sSimplex& simplex,const btVector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } +static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c) + { + return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()- + a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+ + a.x()*b.y()*c.z()-a.z()*b.y()*c.x()); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + btScalar* w,U& m) + { + const btVector3 d=b-a; + const btScalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const btScalar t(l>0?-dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } +static btScalar projectorigin( const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar* w,U& m) + { + static const U imd3[]={1,2,0}; + const btVector3* vt[]={&a,&b,&c}; + const btVector3 dl[]={a-b,b-c,c-a}; + const btVector3 n=cross(dl[0],dl[1]); + const btScalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + btScalar mindist=-1; + btScalar subw[2] = { btScalar(0.0f), btScalar(0.0f) }; + U subm; + for(U i=0;i<3;++i) + { + if(dot(*vt[i],cross(dl[i],n))>0) + { + const U j=imd3[i]; + const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subdGJK_SIMPLEX4_EPS)) + { + btScalar mindist=-1; + btScalar subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const btScalar s=vl*dot(d,cross(dl[i],dl[j])); + if(s>0) + { + const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd1)&&gjk.EncloseOrigin()) + { + /* Clean up */ + while(m_hull.root) + { + sFace* f(m_hull.root); + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + btSwap(simplex.c[0],simplex.c[1]); + btSwap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const btScalar wdist=dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const btVector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const btScalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = btVector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } +sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = cross(b->w-a->w,c->w-a->w); + const btScalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = btMin(btMin( + dot(a->w,cross(face->n,a->w-b->w)), + dot(b->w,cross(face->n,b->w-c->w))), + dot(c->w,cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } +sFace* findbest() + { + sFace* minf=m_hull.root; + btScalar mind=minf->d*minf->d; + btScalar maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const btScalar sqd=f->d*f->d; + if((f->p>=maxp)&&(sqdp; + } + } + return(minf); + } +bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } +static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } +static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } +static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } +}; + +// +static void Initialize(void* shapeA, + SpuConvexPolyhedronVertexData* convexDataA, + int shapeTypeA, + float marginA, + const btTransform& wtrs0, + void* shapeB, + SpuConvexPolyhedronVertexData* convexDataB, + int shapeTypeB, + float marginB, + const btTransform& wtrs1, + SpuGjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) +{ +/* Results */ +results.witnesses[0] = +results.witnesses[1] = btVector3(0,0,0); +results.status = SpuGjkEpaSolver2::sResults::Separated; +/* Shape */ +shape.m_shapes[0].margin = marginA; +shape.m_shapes[0].shape = shapeA; +shape.m_shapes[0].shapeType = shapeTypeA; +shape.m_shapes[0].convexData = convexDataA; +shape.m_shapes[1].margin = marginB; +shape.m_shapes[1].shape = shapeB; +shape.m_shapes[1].shapeType = shapeTypeB; +shape.m_shapes[1].convexData = convexDataB; +shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis()); +shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); +shape.EnableMargin(withmargins); +} + +} + +// +// Api +// + +using namespace gjkepa2_spu_impl; + +// +int SpuGjkEpaSolver2::StackSizeRequirement() +{ +return(sizeof(GJK)+sizeof(EPA)); +} + +// +bool SpuGjkEpaSolver2::Penetration(void* shapeA, + SpuConvexPolyhedronVertexData* convexDataA, + int shapeTypeA, + float marginA, + const btTransform& wtrs0, + void* shapeB, + SpuConvexPolyhedronVertexData* convexDataB, + int shapeTypeB, + float marginB, + const btTransform& wtrs1, + const btVector3& guess, + sResults& results) +{ +tShape shape; +Initialize(shapeA, convexDataA, shapeTypeA, marginA, wtrs0, shapeB, convexDataB, shapeTypeB, marginB, wtrs1, results,shape,true); +GJK gjk; +GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); +switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + btVector3 w0=btVector3(0,0,0); + for(U i=0;id,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth); + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + } +return(false); +} + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp index ad7fc2633..30642a392 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp @@ -1,1155 +1,1155 @@ -/* - Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. - All rights reserved. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - - -#include "Box.h" - -static inline float sqr( float a ) -{ - return (a * a); -} - -enum BoxSepAxisType -{ - A_AXIS, B_AXIS, CROSS_AXIS -}; - -//------------------------------------------------------------------------------------------------- -// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel. -//------------------------------------------------------------------------------------------------- - -static const float voronoiTol = -1.0e-5f; - -//------------------------------------------------------------------------------------------------- -// separating axis tests: gaps along each axis are computed, and the axis with the maximum -// gap is stored. cross product axes are normalized. -//------------------------------------------------------------------------------------------------- - -#define AaxisTest( dim, letter, first ) \ -{ \ - if ( first ) \ - { \ - maxGap = gap = gapsA.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - axisType = A_AXIS; \ - faceDimA = dim; \ - axisA = identity.getCol##dim(); \ - } \ - else \ - { \ - gap = gapsA.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - else if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = A_AXIS; \ - faceDimA = dim; \ - axisA = identity.getCol##dim(); \ - } \ - } \ -} - - -#define BaxisTest( dim, letter ) \ -{ \ - gap = gapsB.get##letter(); \ - if ( gap > distanceThreshold ) return gap; \ - else if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = B_AXIS; \ - faceDimB = dim; \ - axisB = identity.getCol##dim(); \ - } \ -} - -#define CrossAxisTest( dima, dimb, letterb ) \ -{ \ - const float lsqr_tolerance = 1.0e-30f; \ - float lsqr; \ - \ - lsqr = lsqrs.getCol##dima().get##letterb(); \ - \ - if ( lsqr > lsqr_tolerance ) \ - { \ - float l_recip = 1.0f / sqrtf( lsqr ); \ - gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \ - \ - if ( gap > distanceThreshold ) \ - { \ - return gap; \ - } \ - \ - if ( gap > maxGap ) \ - { \ - maxGap = gap; \ - axisType = CROSS_AXIS; \ - edgeDimA = dima; \ - edgeDimB = dimb; \ - axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \ - } \ - } \ -} - -//------------------------------------------------------------------------------------------------- -// tests whether a vertex of box B and a face of box A are the closest features -//------------------------------------------------------------------------------------------------- - -inline -float -VertexBFaceATest( - bool & inVoronoi, - float & t0, - float & t1, - const Vector3 & hA, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesB ) -{ - // compute a corner of box B in A's coordinate system - - Vector3 corner = - Vector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() ); - - // compute the parameters of the point on A, closest to this corner - - t0 = corner[0]; - t1 = corner[1]; - - if ( t0 > hA[0] ) - t0 = hA[0]; - else if ( t0 < -hA[0] ) - t0 = -hA[0]; - if ( t1 > hA[1] ) - t1 = hA[1]; - else if ( t1 < -hA[1] ) - t1 = -hA[1]; - - // do the Voronoi test: already know the point on B is in the Voronoi region of the - // point on A, check the reverse. - - Vector3 facePointB = - Vector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) ); - - inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) && - ( facePointB[1] >= voronoiTol * facePointB[0] ) && - ( facePointB[2] >= voronoiTol * facePointB[1] ) ); - - return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); -} - -#define VertexBFaceA_SetNewMin() \ -{ \ - minDistSqr = distSqr; \ - localPointA.setX(t0); \ - localPointA.setY(t1); \ - localPointB.setX( scalesB.getX() ); \ - localPointB.setY( scalesB.getY() ); \ - featureA = F; \ - featureB = V; \ -} - -void -VertexBFaceATests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hA, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesB, - bool first ) -{ - - float t0, t1; - float distSqr; - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( first ) { - VertexBFaceA_SetNewMin(); - } else { - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsB, scalesB ); - - if ( distSqr < minDistSqr ) { - VertexBFaceA_SetNewMin(); - } -} - -//------------------------------------------------------------------------------------------------- -// VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features -//------------------------------------------------------------------------------------------------- - -inline -float -VertexAFaceBTest( - bool & inVoronoi, - float & t0, - float & t1, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) scalesA ) -{ - Vector3 corner = - Vector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() ); - - t0 = corner[0]; - t1 = corner[1]; - - if ( t0 > hB[0] ) - t0 = hB[0]; - else if ( t0 < -hB[0] ) - t0 = -hB[0]; - if ( t1 > hB[1] ) - t1 = hB[1]; - else if ( t1 < -hB[1] ) - t1 = -hB[1]; - - Vector3 facePointA = - Vector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) ); - - inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) && - ( facePointA[1] >= voronoiTol * facePointA[0] ) && - ( facePointA[2] >= voronoiTol * facePointA[1] ) ); - - return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); -} - -#define VertexAFaceB_SetNewMin() \ -{ \ - minDistSqr = distSqr; \ - localPointB.setX(t0); \ - localPointB.setY(t1); \ - localPointA.setX( scalesA.getX() ); \ - localPointA.setY( scalesA.getY() ); \ - featureA = V; \ - featureB = F; \ -} - -void -VertexAFaceBTests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) scalesA, - bool first ) -{ - float t0, t1; - float distSqr; - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( first ) { - VertexAFaceB_SetNewMin(); - } else { - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, scalesA ); - - if ( distSqr < minDistSqr ) { - VertexAFaceB_SetNewMin(); - } -} - -//------------------------------------------------------------------------------------------------- -// EdgeEdgeTest: -// -// tests whether a pair of edges are the closest features -// -// note on the shorthand: -// 'a' & 'b' refer to the edges. -// 'c' is the dimension of the axis that points from the face center to the edge Center -// 'd' is the dimension of the edge Direction -// the dimension of the face normal is 2 -//------------------------------------------------------------------------------------------------- - -#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \ -{ \ - Vector3 edgeOffsetAB; \ - Vector3 edgeOffsetBA; \ - \ - edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \ - edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \ - \ - edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \ - edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \ - \ - float dirDot = matrixAB.getCol##bd().get##ad_letter(); \ - float denom = 1.0f - dirDot*dirDot; \ - float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \ - float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \ - \ - if ( denom == 0.0f ) \ - { \ - tA = 0.0f; \ - } \ - else \ - { \ - tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \ - } \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - \ - tB = tA * dirDot + edgeOffsetBA_bd; \ - \ - if ( tB < -hB[bd] ) \ - { \ - tB = -hB[bd]; \ - tA = tB * dirDot + edgeOffsetAB_ad; \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - } \ - else if ( tB > hB[bd] ) \ - { \ - tB = hB[bd]; \ - tA = tB * dirDot + edgeOffsetAB_ad; \ - \ - if ( tA < -hA[ad] ) tA = -hA[ad]; \ - else if ( tA > hA[ad] ) tA = hA[ad]; \ - } \ - \ - Vector3 edgeOffAB = Vector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\ - Vector3 edgeOffBA = Vector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\ - \ - inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \ - ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \ - ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \ - ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \ - \ - edgeOffAB[ad] -= tA; \ - edgeOffBA[bd] -= tB; \ - \ - return dot(edgeOffAB,edgeOffAB); \ -} - -float -EdgeEdgeTest_0101( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y ); -} - -float -EdgeEdgeTest_0110( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X ); -} - -float -EdgeEdgeTest_1001( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y ); -} - -float -EdgeEdgeTest_1010( - bool & inVoronoi, - float & tA, - float & tB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB ) -{ - EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X ); -} - -#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \ -{ \ - minDistSqr = distSqr; \ - localPointA.set##ac_letter(scalesA.get##ac_letter()); \ - localPointA.set##ad_letter(tA); \ - localPointB.set##bc_letter(scalesB.get##bc_letter()); \ - localPointB.set##bd_letter(tB); \ - otherFaceDimA = testOtherFaceDimA; \ - otherFaceDimB = testOtherFaceDimB; \ - featureA = E; \ - featureB = E; \ -} - -void -EdgeEdgeTests( - bool & done, - float & minDistSqr, - Point3 & localPointA, - Point3 & localPointB, - int & otherFaceDimA, - int & otherFaceDimB, - FeatureType & featureA, - FeatureType & featureB, - const Vector3 & hA, - const Vector3 & hB, - PE_REF(Vector3) faceOffsetAB, - PE_REF(Vector3) faceOffsetBA, - const Matrix3 & matrixAB, - const Matrix3 & matrixBA, - PE_REF(Vector3) signsA, - PE_REF(Vector3) signsB, - PE_REF(Vector3) scalesA, - PE_REF(Vector3) scalesB, - bool first ) -{ - - float distSqr; - float tA, tB; - - int testOtherFaceDimA, testOtherFaceDimB; - - testOtherFaceDimA = 0; - testOtherFaceDimB = 0; - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( first ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } else { - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, X, Y ); - } - - if ( done ) - return; - - testOtherFaceDimA = 1; - testOtherFaceDimB = 0; - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, X, Y ); - } - - if ( done ) - return; - - testOtherFaceDimA = 0; - testOtherFaceDimB = 1; - signsB.setX( -signsB.getX() ); - scalesB.setX( -scalesB.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - signsA.setX( -signsA.getX() ); - scalesA.setX( -scalesA.getX() ); - - distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( X, Y, Y, X ); - } - - if ( done ) - return; - - testOtherFaceDimA = 1; - testOtherFaceDimB = 1; - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsB.setY( -signsB.getY() ); - scalesB.setY( -scalesB.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } - - if ( done ) - return; - - signsA.setY( -signsA.getY() ); - scalesA.setY( -scalesA.getY() ); - - distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, - matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); - - if ( distSqr < minDistSqr ) { - EdgeEdge_SetNewMin( Y, X, Y, X ); - } -} - -float -boxBoxDistance( - Vector3& normal, - BoxPoint& boxPointA, - BoxPoint& boxPointB, - PE_REF(Box) boxA, const Transform3& transformA, - PE_REF(Box) boxB, const Transform3& transformB, - float distanceThreshold ) -{ - Matrix3 identity; - identity = Matrix3::identity(); - Vector3 ident[3]; - ident[0] = identity.getCol0(); - ident[1] = identity.getCol1(); - ident[2] = identity.getCol2(); - - // get relative transformations - - Transform3 transformAB, transformBA; - Matrix3 matrixAB, matrixBA; - Vector3 offsetAB, offsetBA; - - transformAB = orthoInverse(transformA) * transformB; - transformBA = orthoInverse(transformAB); - - matrixAB = transformAB.getUpper3x3(); - offsetAB = transformAB.getTranslation(); - matrixBA = transformBA.getUpper3x3(); - offsetBA = transformBA.getTranslation(); - - Matrix3 absMatrixAB = absPerElem(matrixAB); - Matrix3 absMatrixBA = absPerElem(matrixBA); - - // find separating axis with largest gap between projections - - BoxSepAxisType axisType; - Vector3 axisA(0.0f), axisB(0.0f); - float gap, maxGap; - int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0; - - // face axes - - Vector3 gapsA = absPerElem(offsetAB) - boxA.half - absMatrixAB * boxB.half; - - AaxisTest(0,X,true); - AaxisTest(1,Y,false); - AaxisTest(2,Z,false); - - Vector3 gapsB = absPerElem(offsetBA) - boxB.half - absMatrixBA * boxA.half; - - BaxisTest(0,X); - BaxisTest(1,Y); - BaxisTest(2,Z); - - // cross product axes - - // ŠOÏ‚ª‚O‚Ì‚Æ‚«‚Ì‘Îô - absMatrixAB += Matrix3(1.0e-5f); - absMatrixBA += Matrix3(1.0e-5f); - - Matrix3 lsqrs, projOffset, projAhalf, projBhalf; - - lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + - mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) ); - lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + - mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); - lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) + - mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); - - projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY()); - projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ()); - projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX()); - - projAhalf.setCol0(absMatrixBA.getCol1() * boxA.half.getZ() + absMatrixBA.getCol2() * boxA.half.getY()); - projAhalf.setCol1(absMatrixBA.getCol2() * boxA.half.getX() + absMatrixBA.getCol0() * boxA.half.getZ()); - projAhalf.setCol2(absMatrixBA.getCol0() * boxA.half.getY() + absMatrixBA.getCol1() * boxA.half.getX()); - - projBhalf.setCol0(absMatrixAB.getCol1() * boxB.half.getZ() + absMatrixAB.getCol2() * boxB.half.getY()); - projBhalf.setCol1(absMatrixAB.getCol2() * boxB.half.getX() + absMatrixAB.getCol0() * boxB.half.getZ()); - projBhalf.setCol2(absMatrixAB.getCol0() * boxB.half.getY() + absMatrixAB.getCol1() * boxB.half.getX()); - - Matrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf); - - CrossAxisTest(0,0,X); - CrossAxisTest(0,1,Y); - CrossAxisTest(0,2,Z); - CrossAxisTest(1,0,X); - CrossAxisTest(1,1,Y); - CrossAxisTest(1,2,Z); - CrossAxisTest(2,0,X); - CrossAxisTest(2,1,Y); - CrossAxisTest(2,2,Z); - - // need to pick the face on each box whose normal best matches the separating axis. - // will transform vectors to be in the coordinate system of this face to simplify things later. - // for this, a permutation matrix can be used, which the next section computes. - - int dimA[3], dimB[3]; - - if ( axisType == A_AXIS ) { - if ( dot(axisA,offsetAB) < 0.0f ) - axisA = -axisA; - axisB = matrixBA * -axisA; - - Vector3 absAxisB = Vector3(absPerElem(axisB)); - - if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) ) - faceDimB = 0; - else if ( absAxisB[1] > absAxisB[2] ) - faceDimB = 1; - else - faceDimB = 2; - } else if ( axisType == B_AXIS ) { - if ( dot(axisB,offsetBA) < 0.0f ) - axisB = -axisB; - axisA = matrixAB * -axisB; - - Vector3 absAxisA = Vector3(absPerElem(axisA)); - - if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) ) - faceDimA = 0; - else if ( absAxisA[1] > absAxisA[2] ) - faceDimA = 1; - else - faceDimA = 2; - } - - if ( axisType == CROSS_AXIS ) { - if ( dot(axisA,offsetAB) < 0.0f ) - axisA = -axisA; - axisB = matrixBA * -axisA; - - Vector3 absAxisA = Vector3(absPerElem(axisA)); - Vector3 absAxisB = Vector3(absPerElem(axisB)); - - dimA[1] = edgeDimA; - dimB[1] = edgeDimB; - - if ( edgeDimA == 0 ) { - if ( absAxisA[1] > absAxisA[2] ) { - dimA[0] = 2; - dimA[2] = 1; - } else { - dimA[0] = 1; - dimA[2] = 2; - } - } else if ( edgeDimA == 1 ) { - if ( absAxisA[2] > absAxisA[0] ) { - dimA[0] = 0; - dimA[2] = 2; - } else { - dimA[0] = 2; - dimA[2] = 0; - } - } else { - if ( absAxisA[0] > absAxisA[1] ) { - dimA[0] = 1; - dimA[2] = 0; - } else { - dimA[0] = 0; - dimA[2] = 1; - } - } - - if ( edgeDimB == 0 ) { - if ( absAxisB[1] > absAxisB[2] ) { - dimB[0] = 2; - dimB[2] = 1; - } else { - dimB[0] = 1; - dimB[2] = 2; - } - } else if ( edgeDimB == 1 ) { - if ( absAxisB[2] > absAxisB[0] ) { - dimB[0] = 0; - dimB[2] = 2; - } else { - dimB[0] = 2; - dimB[2] = 0; - } - } else { - if ( absAxisB[0] > absAxisB[1] ) { - dimB[0] = 1; - dimB[2] = 0; - } else { - dimB[0] = 0; - dimB[2] = 1; - } - } - } else { - dimA[2] = faceDimA; - dimA[0] = (faceDimA+1)%3; - dimA[1] = (faceDimA+2)%3; - dimB[2] = faceDimB; - dimB[0] = (faceDimB+1)%3; - dimB[1] = (faceDimB+2)%3; - } - - Matrix3 aperm_col, bperm_col; - - aperm_col.setCol0(ident[dimA[0]]); - aperm_col.setCol1(ident[dimA[1]]); - aperm_col.setCol2(ident[dimA[2]]); - - bperm_col.setCol0(ident[dimB[0]]); - bperm_col.setCol1(ident[dimB[1]]); - bperm_col.setCol2(ident[dimB[2]]); - - Matrix3 aperm_row, bperm_row; - - aperm_row = transpose(aperm_col); - bperm_row = transpose(bperm_col); - - // permute all box parameters to be in the face coordinate systems - - Matrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col; - Matrix3 matrixBA_perm = transpose(matrixAB_perm); - - Vector3 offsetAB_perm, offsetBA_perm; - - offsetAB_perm = aperm_row * offsetAB; - offsetBA_perm = bperm_row * offsetBA; - - Vector3 halfA_perm, halfB_perm; - - halfA_perm = aperm_row * boxA.half; - halfB_perm = bperm_row * boxB.half; - - // compute the vector between the centers of each face, in each face's coordinate frame - - Vector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm; - - signsA_perm = copySignPerElem(Vector3(1.0f),aperm_row * axisA); - signsB_perm = copySignPerElem(Vector3(1.0f),bperm_row * axisB); - scalesA_perm = mulPerElem( signsA_perm, halfA_perm ); - scalesB_perm = mulPerElem( signsB_perm, halfB_perm ); - - faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ(); - faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() ); - - faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ(); - faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() ); - - if ( maxGap < 0.0f ) { - // if boxes overlap, this will separate the faces for finding points of penetration. - - faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f; - faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f; - } - - // for each vertex/face or edge/edge pair of the two faces, find the closest points. - // - // these points each have an associated box feature (vertex, edge, or face). if each - // point is in the external Voronoi region of the other's feature, they are the - // closest points of the boxes, and the algorithm can exit. - // - // the feature pairs are arranged so that in the general case, the first test will - // succeed. degenerate cases (parallel faces) may require up to all tests in the - // worst case. - // - // if for some reason no case passes the Voronoi test, the features with the minimum - // distance are returned. - - Point3 localPointA_perm, localPointB_perm; - float minDistSqr; - bool done; - - Vector3 hA_perm( halfA_perm ), hB_perm( halfB_perm ); - - localPointA_perm.setZ( scalesA_perm.getZ() ); - localPointB_perm.setZ( scalesB_perm.getZ() ); - scalesA_perm.setZ(0.0f); - scalesB_perm.setZ(0.0f); - - int otherFaceDimA, otherFaceDimB; - FeatureType featureA, featureB; - - if ( axisType == CROSS_AXIS ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, true ); - - if ( !done ) { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); - - if ( !done ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); - } - } - } else if ( axisType == B_AXIS ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true ); - - if ( !done ) { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); - - if ( !done ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, false ); - } - } - } else { - VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true ); - - if ( !done ) { - VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, - featureA, featureB, - hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); - - if ( !done ) { - EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, - otherFaceDimA, otherFaceDimB, featureA, featureB, - hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, - matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, - scalesA_perm, scalesB_perm, false ); - } - } - } - - // convert local points from face-local to box-local coordinate system - - boxPointA.localPoint = Point3( aperm_col * Vector3( localPointA_perm ) ); - boxPointB.localPoint = Point3( bperm_col * Vector3( localPointB_perm ) ); - - // find which features of the boxes are involved. - // the only feature pairs which occur in this function are VF, FV, and EE, even though the - // closest points might actually lie on sub-features, as in a VF contact might be used for - // what's actually a VV contact. this means some feature pairs could possibly seem distinct - // from others, although their contact positions are the same. don't know yet whether this - // matters. - - int sA[3], sB[3]; - - sA[0] = boxPointA.localPoint.getX() > 0.0f; - sA[1] = boxPointA.localPoint.getY() > 0.0f; - sA[2] = boxPointA.localPoint.getZ() > 0.0f; - - sB[0] = boxPointB.localPoint.getX() > 0.0f; - sB[1] = boxPointB.localPoint.getY() > 0.0f; - sB[2] = boxPointB.localPoint.getZ() > 0.0f; - - if ( featureA == F ) { - boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] ); - } else if ( featureA == E ) { - boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] ); - } else { - boxPointA.setVertexFeature( sA[0], sA[1], sA[2] ); - } - - if ( featureB == F ) { - boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] ); - } else if ( featureB == E ) { - boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] ); - } else { - boxPointB.setVertexFeature( sB[0], sB[1], sB[2] ); - } - - normal = transformA * axisA; - - if ( maxGap < 0.0f ) { - return (maxGap); - } else { - return (sqrtf( minDistSqr )); - } -} +/* + Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. + All rights reserved. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + + +#include "Box.h" + +static inline float sqr( float a ) +{ + return (a * a); +} + +enum BoxSepAxisType +{ + A_AXIS, B_AXIS, CROSS_AXIS +}; + +//------------------------------------------------------------------------------------------------- +// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel. +//------------------------------------------------------------------------------------------------- + +static const float voronoiTol = -1.0e-5f; + +//------------------------------------------------------------------------------------------------- +// separating axis tests: gaps along each axis are computed, and the axis with the maximum +// gap is stored. cross product axes are normalized. +//------------------------------------------------------------------------------------------------- + +#define AaxisTest( dim, letter, first ) \ +{ \ + if ( first ) \ + { \ + maxGap = gap = gapsA.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + axisType = A_AXIS; \ + faceDimA = dim; \ + axisA = identity.getCol##dim(); \ + } \ + else \ + { \ + gap = gapsA.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + else if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = A_AXIS; \ + faceDimA = dim; \ + axisA = identity.getCol##dim(); \ + } \ + } \ +} + + +#define BaxisTest( dim, letter ) \ +{ \ + gap = gapsB.get##letter(); \ + if ( gap > distanceThreshold ) return gap; \ + else if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = B_AXIS; \ + faceDimB = dim; \ + axisB = identity.getCol##dim(); \ + } \ +} + +#define CrossAxisTest( dima, dimb, letterb ) \ +{ \ + const float lsqr_tolerance = 1.0e-30f; \ + float lsqr; \ + \ + lsqr = lsqrs.getCol##dima().get##letterb(); \ + \ + if ( lsqr > lsqr_tolerance ) \ + { \ + float l_recip = 1.0f / sqrtf( lsqr ); \ + gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \ + \ + if ( gap > distanceThreshold ) \ + { \ + return gap; \ + } \ + \ + if ( gap > maxGap ) \ + { \ + maxGap = gap; \ + axisType = CROSS_AXIS; \ + edgeDimA = dima; \ + edgeDimB = dimb; \ + axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \ + } \ + } \ +} + +//------------------------------------------------------------------------------------------------- +// tests whether a vertex of box B and a face of box A are the closest features +//------------------------------------------------------------------------------------------------- + +inline +float +VertexBFaceATest( + bool & inVoronoi, + float & t0, + float & t1, + const Vector3 & hA, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesB ) +{ + // compute a corner of box B in A's coordinate system + + Vector3 corner = + Vector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() ); + + // compute the parameters of the point on A, closest to this corner + + t0 = corner[0]; + t1 = corner[1]; + + if ( t0 > hA[0] ) + t0 = hA[0]; + else if ( t0 < -hA[0] ) + t0 = -hA[0]; + if ( t1 > hA[1] ) + t1 = hA[1]; + else if ( t1 < -hA[1] ) + t1 = -hA[1]; + + // do the Voronoi test: already know the point on B is in the Voronoi region of the + // point on A, check the reverse. + + Vector3 facePointB = + Vector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) ); + + inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) && + ( facePointB[1] >= voronoiTol * facePointB[0] ) && + ( facePointB[2] >= voronoiTol * facePointB[1] ) ); + + return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); +} + +#define VertexBFaceA_SetNewMin() \ +{ \ + minDistSqr = distSqr; \ + localPointA.setX(t0); \ + localPointA.setY(t1); \ + localPointB.setX( scalesB.getX() ); \ + localPointB.setY( scalesB.getY() ); \ + featureA = F; \ + featureB = V; \ +} + +void +VertexBFaceATests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hA, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesB, + bool first ) +{ + + float t0, t1; + float distSqr; + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( first ) { + VertexBFaceA_SetNewMin(); + } else { + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsB, scalesB ); + + if ( distSqr < minDistSqr ) { + VertexBFaceA_SetNewMin(); + } +} + +//------------------------------------------------------------------------------------------------- +// VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features +//------------------------------------------------------------------------------------------------- + +inline +float +VertexAFaceBTest( + bool & inVoronoi, + float & t0, + float & t1, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) scalesA ) +{ + Vector3 corner = + Vector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() ); + + t0 = corner[0]; + t1 = corner[1]; + + if ( t0 > hB[0] ) + t0 = hB[0]; + else if ( t0 < -hB[0] ) + t0 = -hB[0]; + if ( t1 > hB[1] ) + t1 = hB[1]; + else if ( t1 < -hB[1] ) + t1 = -hB[1]; + + Vector3 facePointA = + Vector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) ); + + inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) && + ( facePointA[1] >= voronoiTol * facePointA[0] ) && + ( facePointA[2] >= voronoiTol * facePointA[1] ) ); + + return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] )); +} + +#define VertexAFaceB_SetNewMin() \ +{ \ + minDistSqr = distSqr; \ + localPointB.setX(t0); \ + localPointB.setY(t1); \ + localPointA.setX( scalesA.getX() ); \ + localPointA.setY( scalesA.getY() ); \ + featureA = V; \ + featureB = F; \ +} + +void +VertexAFaceBTests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) scalesA, + bool first ) +{ + float t0, t1; + float distSqr; + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( first ) { + VertexAFaceB_SetNewMin(); + } else { + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, scalesA ); + + if ( distSqr < minDistSqr ) { + VertexAFaceB_SetNewMin(); + } +} + +//------------------------------------------------------------------------------------------------- +// EdgeEdgeTest: +// +// tests whether a pair of edges are the closest features +// +// note on the shorthand: +// 'a' & 'b' refer to the edges. +// 'c' is the dimension of the axis that points from the face center to the edge Center +// 'd' is the dimension of the edge Direction +// the dimension of the face normal is 2 +//------------------------------------------------------------------------------------------------- + +#define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \ +{ \ + Vector3 edgeOffsetAB; \ + Vector3 edgeOffsetBA; \ + \ + edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \ + edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \ + \ + edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \ + edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \ + \ + float dirDot = matrixAB.getCol##bd().get##ad_letter(); \ + float denom = 1.0f - dirDot*dirDot; \ + float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \ + float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \ + \ + if ( denom == 0.0f ) \ + { \ + tA = 0.0f; \ + } \ + else \ + { \ + tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \ + } \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + \ + tB = tA * dirDot + edgeOffsetBA_bd; \ + \ + if ( tB < -hB[bd] ) \ + { \ + tB = -hB[bd]; \ + tA = tB * dirDot + edgeOffsetAB_ad; \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + } \ + else if ( tB > hB[bd] ) \ + { \ + tB = hB[bd]; \ + tA = tB * dirDot + edgeOffsetAB_ad; \ + \ + if ( tA < -hA[ad] ) tA = -hA[ad]; \ + else if ( tA > hA[ad] ) tA = hA[ad]; \ + } \ + \ + Vector3 edgeOffAB = Vector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\ + Vector3 edgeOffBA = Vector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\ + \ + inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \ + ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \ + ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \ + ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \ + \ + edgeOffAB[ad] -= tA; \ + edgeOffBA[bd] -= tB; \ + \ + return dot(edgeOffAB,edgeOffAB); \ +} + +float +EdgeEdgeTest_0101( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y ); +} + +float +EdgeEdgeTest_0110( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X ); +} + +float +EdgeEdgeTest_1001( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y ); +} + +float +EdgeEdgeTest_1010( + bool & inVoronoi, + float & tA, + float & tB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB ) +{ + EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X ); +} + +#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \ +{ \ + minDistSqr = distSqr; \ + localPointA.set##ac_letter(scalesA.get##ac_letter()); \ + localPointA.set##ad_letter(tA); \ + localPointB.set##bc_letter(scalesB.get##bc_letter()); \ + localPointB.set##bd_letter(tB); \ + otherFaceDimA = testOtherFaceDimA; \ + otherFaceDimB = testOtherFaceDimB; \ + featureA = E; \ + featureB = E; \ +} + +void +EdgeEdgeTests( + bool & done, + float & minDistSqr, + Point3 & localPointA, + Point3 & localPointB, + int & otherFaceDimA, + int & otherFaceDimB, + FeatureType & featureA, + FeatureType & featureB, + const Vector3 & hA, + const Vector3 & hB, + PE_REF(Vector3) faceOffsetAB, + PE_REF(Vector3) faceOffsetBA, + const Matrix3 & matrixAB, + const Matrix3 & matrixBA, + PE_REF(Vector3) signsA, + PE_REF(Vector3) signsB, + PE_REF(Vector3) scalesA, + PE_REF(Vector3) scalesB, + bool first ) +{ + + float distSqr; + float tA, tB; + + int testOtherFaceDimA, testOtherFaceDimB; + + testOtherFaceDimA = 0; + testOtherFaceDimB = 0; + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( first ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } else { + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, X, Y ); + } + + if ( done ) + return; + + testOtherFaceDimA = 1; + testOtherFaceDimB = 0; + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, X, Y ); + } + + if ( done ) + return; + + testOtherFaceDimA = 0; + testOtherFaceDimB = 1; + signsB.setX( -signsB.getX() ); + scalesB.setX( -scalesB.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + signsA.setX( -signsA.getX() ); + scalesA.setX( -scalesA.getX() ); + + distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( X, Y, Y, X ); + } + + if ( done ) + return; + + testOtherFaceDimA = 1; + testOtherFaceDimB = 1; + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsB.setY( -signsB.getY() ); + scalesB.setY( -scalesB.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } + + if ( done ) + return; + + signsA.setY( -signsA.getY() ); + scalesA.setY( -scalesA.getY() ); + + distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA, + matrixAB, matrixBA, signsA, signsB, scalesA, scalesB ); + + if ( distSqr < minDistSqr ) { + EdgeEdge_SetNewMin( Y, X, Y, X ); + } +} + +float +boxBoxDistance( + Vector3& normal, + BoxPoint& boxPointA, + BoxPoint& boxPointB, + PE_REF(Box) boxA, const Transform3& transformA, + PE_REF(Box) boxB, const Transform3& transformB, + float distanceThreshold ) +{ + Matrix3 identity; + identity = Matrix3::identity(); + Vector3 ident[3]; + ident[0] = identity.getCol0(); + ident[1] = identity.getCol1(); + ident[2] = identity.getCol2(); + + // get relative transformations + + Transform3 transformAB, transformBA; + Matrix3 matrixAB, matrixBA; + Vector3 offsetAB, offsetBA; + + transformAB = orthoInverse(transformA) * transformB; + transformBA = orthoInverse(transformAB); + + matrixAB = transformAB.getUpper3x3(); + offsetAB = transformAB.getTranslation(); + matrixBA = transformBA.getUpper3x3(); + offsetBA = transformBA.getTranslation(); + + Matrix3 absMatrixAB = absPerElem(matrixAB); + Matrix3 absMatrixBA = absPerElem(matrixBA); + + // find separating axis with largest gap between projections + + BoxSepAxisType axisType; + Vector3 axisA(0.0f), axisB(0.0f); + float gap, maxGap; + int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0; + + // face axes + + Vector3 gapsA = absPerElem(offsetAB) - boxA.half - absMatrixAB * boxB.half; + + AaxisTest(0,X,true); + AaxisTest(1,Y,false); + AaxisTest(2,Z,false); + + Vector3 gapsB = absPerElem(offsetBA) - boxB.half - absMatrixBA * boxA.half; + + BaxisTest(0,X); + BaxisTest(1,Y); + BaxisTest(2,Z); + + // cross product axes + + // ŠOÏ‚ª‚O‚Ì‚Æ‚«‚Ì‘Îô + absMatrixAB += Matrix3(1.0e-5f); + absMatrixBA += Matrix3(1.0e-5f); + + Matrix3 lsqrs, projOffset, projAhalf, projBhalf; + + lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + + mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) ); + lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) + + mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); + lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) + + mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) ); + + projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY()); + projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ()); + projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX()); + + projAhalf.setCol0(absMatrixBA.getCol1() * boxA.half.getZ() + absMatrixBA.getCol2() * boxA.half.getY()); + projAhalf.setCol1(absMatrixBA.getCol2() * boxA.half.getX() + absMatrixBA.getCol0() * boxA.half.getZ()); + projAhalf.setCol2(absMatrixBA.getCol0() * boxA.half.getY() + absMatrixBA.getCol1() * boxA.half.getX()); + + projBhalf.setCol0(absMatrixAB.getCol1() * boxB.half.getZ() + absMatrixAB.getCol2() * boxB.half.getY()); + projBhalf.setCol1(absMatrixAB.getCol2() * boxB.half.getX() + absMatrixAB.getCol0() * boxB.half.getZ()); + projBhalf.setCol2(absMatrixAB.getCol0() * boxB.half.getY() + absMatrixAB.getCol1() * boxB.half.getX()); + + Matrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf); + + CrossAxisTest(0,0,X); + CrossAxisTest(0,1,Y); + CrossAxisTest(0,2,Z); + CrossAxisTest(1,0,X); + CrossAxisTest(1,1,Y); + CrossAxisTest(1,2,Z); + CrossAxisTest(2,0,X); + CrossAxisTest(2,1,Y); + CrossAxisTest(2,2,Z); + + // need to pick the face on each box whose normal best matches the separating axis. + // will transform vectors to be in the coordinate system of this face to simplify things later. + // for this, a permutation matrix can be used, which the next section computes. + + int dimA[3], dimB[3]; + + if ( axisType == A_AXIS ) { + if ( dot(axisA,offsetAB) < 0.0f ) + axisA = -axisA; + axisB = matrixBA * -axisA; + + Vector3 absAxisB = Vector3(absPerElem(axisB)); + + if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) ) + faceDimB = 0; + else if ( absAxisB[1] > absAxisB[2] ) + faceDimB = 1; + else + faceDimB = 2; + } else if ( axisType == B_AXIS ) { + if ( dot(axisB,offsetBA) < 0.0f ) + axisB = -axisB; + axisA = matrixAB * -axisB; + + Vector3 absAxisA = Vector3(absPerElem(axisA)); + + if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) ) + faceDimA = 0; + else if ( absAxisA[1] > absAxisA[2] ) + faceDimA = 1; + else + faceDimA = 2; + } + + if ( axisType == CROSS_AXIS ) { + if ( dot(axisA,offsetAB) < 0.0f ) + axisA = -axisA; + axisB = matrixBA * -axisA; + + Vector3 absAxisA = Vector3(absPerElem(axisA)); + Vector3 absAxisB = Vector3(absPerElem(axisB)); + + dimA[1] = edgeDimA; + dimB[1] = edgeDimB; + + if ( edgeDimA == 0 ) { + if ( absAxisA[1] > absAxisA[2] ) { + dimA[0] = 2; + dimA[2] = 1; + } else { + dimA[0] = 1; + dimA[2] = 2; + } + } else if ( edgeDimA == 1 ) { + if ( absAxisA[2] > absAxisA[0] ) { + dimA[0] = 0; + dimA[2] = 2; + } else { + dimA[0] = 2; + dimA[2] = 0; + } + } else { + if ( absAxisA[0] > absAxisA[1] ) { + dimA[0] = 1; + dimA[2] = 0; + } else { + dimA[0] = 0; + dimA[2] = 1; + } + } + + if ( edgeDimB == 0 ) { + if ( absAxisB[1] > absAxisB[2] ) { + dimB[0] = 2; + dimB[2] = 1; + } else { + dimB[0] = 1; + dimB[2] = 2; + } + } else if ( edgeDimB == 1 ) { + if ( absAxisB[2] > absAxisB[0] ) { + dimB[0] = 0; + dimB[2] = 2; + } else { + dimB[0] = 2; + dimB[2] = 0; + } + } else { + if ( absAxisB[0] > absAxisB[1] ) { + dimB[0] = 1; + dimB[2] = 0; + } else { + dimB[0] = 0; + dimB[2] = 1; + } + } + } else { + dimA[2] = faceDimA; + dimA[0] = (faceDimA+1)%3; + dimA[1] = (faceDimA+2)%3; + dimB[2] = faceDimB; + dimB[0] = (faceDimB+1)%3; + dimB[1] = (faceDimB+2)%3; + } + + Matrix3 aperm_col, bperm_col; + + aperm_col.setCol0(ident[dimA[0]]); + aperm_col.setCol1(ident[dimA[1]]); + aperm_col.setCol2(ident[dimA[2]]); + + bperm_col.setCol0(ident[dimB[0]]); + bperm_col.setCol1(ident[dimB[1]]); + bperm_col.setCol2(ident[dimB[2]]); + + Matrix3 aperm_row, bperm_row; + + aperm_row = transpose(aperm_col); + bperm_row = transpose(bperm_col); + + // permute all box parameters to be in the face coordinate systems + + Matrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col; + Matrix3 matrixBA_perm = transpose(matrixAB_perm); + + Vector3 offsetAB_perm, offsetBA_perm; + + offsetAB_perm = aperm_row * offsetAB; + offsetBA_perm = bperm_row * offsetBA; + + Vector3 halfA_perm, halfB_perm; + + halfA_perm = aperm_row * boxA.half; + halfB_perm = bperm_row * boxB.half; + + // compute the vector between the centers of each face, in each face's coordinate frame + + Vector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm; + + signsA_perm = copySignPerElem(Vector3(1.0f),aperm_row * axisA); + signsB_perm = copySignPerElem(Vector3(1.0f),bperm_row * axisB); + scalesA_perm = mulPerElem( signsA_perm, halfA_perm ); + scalesB_perm = mulPerElem( signsB_perm, halfB_perm ); + + faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ(); + faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() ); + + faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ(); + faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() ); + + if ( maxGap < 0.0f ) { + // if boxes overlap, this will separate the faces for finding points of penetration. + + faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f; + faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f; + } + + // for each vertex/face or edge/edge pair of the two faces, find the closest points. + // + // these points each have an associated box feature (vertex, edge, or face). if each + // point is in the external Voronoi region of the other's feature, they are the + // closest points of the boxes, and the algorithm can exit. + // + // the feature pairs are arranged so that in the general case, the first test will + // succeed. degenerate cases (parallel faces) may require up to all tests in the + // worst case. + // + // if for some reason no case passes the Voronoi test, the features with the minimum + // distance are returned. + + Point3 localPointA_perm, localPointB_perm; + float minDistSqr; + bool done; + + Vector3 hA_perm( halfA_perm ), hB_perm( halfB_perm ); + + localPointA_perm.setZ( scalesA_perm.getZ() ); + localPointB_perm.setZ( scalesB_perm.getZ() ); + scalesA_perm.setZ(0.0f); + scalesB_perm.setZ(0.0f); + + int otherFaceDimA, otherFaceDimB; + FeatureType featureA, featureB; + + if ( axisType == CROSS_AXIS ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, true ); + + if ( !done ) { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); + + if ( !done ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); + } + } + } else if ( axisType == B_AXIS ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true ); + + if ( !done ) { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false ); + + if ( !done ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, false ); + } + } + } else { + VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hA_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true ); + + if ( !done ) { + VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm, + featureA, featureB, + hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false ); + + if ( !done ) { + EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm, + otherFaceDimA, otherFaceDimB, featureA, featureB, + hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm, + matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm, + scalesA_perm, scalesB_perm, false ); + } + } + } + + // convert local points from face-local to box-local coordinate system + + boxPointA.localPoint = Point3( aperm_col * Vector3( localPointA_perm ) ); + boxPointB.localPoint = Point3( bperm_col * Vector3( localPointB_perm ) ); + + // find which features of the boxes are involved. + // the only feature pairs which occur in this function are VF, FV, and EE, even though the + // closest points might actually lie on sub-features, as in a VF contact might be used for + // what's actually a VV contact. this means some feature pairs could possibly seem distinct + // from others, although their contact positions are the same. don't know yet whether this + // matters. + + int sA[3], sB[3]; + + sA[0] = boxPointA.localPoint.getX() > 0.0f; + sA[1] = boxPointA.localPoint.getY() > 0.0f; + sA[2] = boxPointA.localPoint.getZ() > 0.0f; + + sB[0] = boxPointB.localPoint.getX() > 0.0f; + sB[1] = boxPointB.localPoint.getY() > 0.0f; + sB[2] = boxPointB.localPoint.getZ() > 0.0f; + + if ( featureA == F ) { + boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] ); + } else if ( featureA == E ) { + boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] ); + } else { + boxPointA.setVertexFeature( sA[0], sA[1], sA[2] ); + } + + if ( featureB == F ) { + boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] ); + } else if ( featureB == E ) { + boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] ); + } else { + boxPointB.setVertexFeature( sB[0], sB[1], sB[2] ); + } + + normal = transformA * axisA; + + if ( maxGap < 0.0f ) { + return (maxGap); + } else { + return (sqrtf( minDistSqr )); + } +} diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h index 7e8b40f75..c58e257c0 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h @@ -1,66 +1,66 @@ -/* - Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. - All rights reserved. - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - - -#ifndef __BOXBOXDISTANCE_H__ -#define __BOXBOXDISTANCE_H__ - - -#include "Box.h" - -using namespace Vectormath::Aos; - -//--------------------------------------------------------------------------- -// boxBoxDistance: -// -// description: -// this computes info that can be used for the collision response of two boxes. when the boxes -// do not overlap, the points are set to the closest points of the boxes, and a positive -// distance between them is returned. if the boxes do overlap, a negative distance is returned -// and the points are set to two points that would touch after the boxes are translated apart. -// the contact normal gives the direction to repel or separate the boxes when they touch or -// overlap (it's being approximated here as one of the 15 "separating axis" directions). -// -// returns: -// positive or negative distance between two boxes. -// -// args: -// Vector3& normal: set to a unit contact normal pointing from box A to box B. -// -// BoxPoint& boxPointA, BoxPoint& boxPointB: -// set to a closest point or point of penetration on each box. -// -// Box boxA, Box boxB: -// boxes, represented as 3 half-widths -// -// const Transform3& transformA, const Transform3& transformB: -// box transformations, in world coordinates -// -// float distanceThreshold: -// the algorithm will exit early if it finds that the boxes are more distant than this -// threshold, and not compute a contact normal or points. if this distance returned -// exceeds the threshold, all the other output data may not have been computed. by -// default, this is set to MAX_FLOAT so it will have no effect. -// -//--------------------------------------------------------------------------- - -float -boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB, - PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB, - const Transform3 & transformB, - float distanceThreshold = FLT_MAX ); - -#endif /* __BOXBOXDISTANCE_H__ */ +/* + Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. + All rights reserved. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + + +#ifndef __BOXBOXDISTANCE_H__ +#define __BOXBOXDISTANCE_H__ + + +#include "Box.h" + +using namespace Vectormath::Aos; + +//--------------------------------------------------------------------------- +// boxBoxDistance: +// +// description: +// this computes info that can be used for the collision response of two boxes. when the boxes +// do not overlap, the points are set to the closest points of the boxes, and a positive +// distance between them is returned. if the boxes do overlap, a negative distance is returned +// and the points are set to two points that would touch after the boxes are translated apart. +// the contact normal gives the direction to repel or separate the boxes when they touch or +// overlap (it's being approximated here as one of the 15 "separating axis" directions). +// +// returns: +// positive or negative distance between two boxes. +// +// args: +// Vector3& normal: set to a unit contact normal pointing from box A to box B. +// +// BoxPoint& boxPointA, BoxPoint& boxPointB: +// set to a closest point or point of penetration on each box. +// +// Box boxA, Box boxB: +// boxes, represented as 3 half-widths +// +// const Transform3& transformA, const Transform3& transformB: +// box transformations, in world coordinates +// +// float distanceThreshold: +// the algorithm will exit early if it finds that the boxes are more distant than this +// threshold, and not compute a contact normal or points. if this distance returned +// exceeds the threshold, all the other output data may not have been computed. by +// default, this is set to MAX_FLOAT so it will have no effect. +// +//--------------------------------------------------------------------------- + +float +boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB, + PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB, + const Transform3 & transformB, + float distanceThreshold = FLT_MAX ); + +#endif /* __BOXBOXDISTANCE_H__ */ diff --git a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt index f3e907347..5b4a90705 100644 --- a/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt +++ b/src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt @@ -1 +1 @@ -Empty placeholder for future Libspe2 SPU task +Empty placeholder for future Libspe2 SPU task diff --git a/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp b/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp index 29170cbcc..8ebbf2e42 100644 --- a/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp +++ b/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp @@ -1,786 +1,786 @@ - - -#include "../PlatformDefinitions.h" -#include "SpuRaycastTask.h" -#include "../SpuCollisionObjectWrapper.h" -#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h" -#include "SpuSubSimplexConvexCast.h" -#include "LinearMath/btAabbUtil2.h" - - -/* Future optimization strategies: -1. BBOX prune before loading shape data -2. Could reduce number of dmas for ray output data to a single read and write. - By sharing the temporary work unit output structures across objects. -3. The reason SpuRaycastNodeCallback1 is slower is because the triangle data isn't - being cached across calls. Fix that by doing the final ray pruning inside the callback. -*/ - -/* Future work: -1. support first hit, closest hit, etc rather than just closest hit. -2. support compound objects -*/ - -#define CALLBACK_ALL - -struct RaycastTask_LocalStoreMemory -{ - ATTRIBUTE_ALIGNED16(char gColObj [sizeof(btCollisionObject)+16]); - btCollisionObject* getColObj() - { - return (btCollisionObject*) gColObj; - } - - ATTRIBUTE_ALIGNED16(SpuCollisionObjectWrapper gCollisionObjectWrapper); - SpuCollisionObjectWrapper* getCollisionObjectWrapper () - { - return &gCollisionObjectWrapper; - } - - CollisionShape_LocalStoreMemory gCollisionShape; - ATTRIBUTE_ALIGNED16(int spuIndices[16]); - - bvhMeshShape_LocalStoreMemory bvhShapeData; - SpuConvexPolyhedronVertexData convexVertexData; - CompoundShape_LocalStoreMemory compoundShapeData; -}; - -#ifdef WIN32 -void* createRaycastLocalStoreMemory() -{ - return new RaycastTask_LocalStoreMemory; -}; -#elif defined(__CELLOS_LV2__) -ATTRIBUTE_ALIGNED16(RaycastTask_LocalStoreMemory gLocalStoreMemory); -void* createRaycastLocalStoreMemory() -{ - return &gLocalStoreMemory; -} -#endif - -void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjectData, RaycastTask_LocalStoreMemory* lsMemPtr, ppu_address_t objectWrapper) -{ - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - /* DMA Collision object wrapper into local store */ - dmaSize = sizeof(SpuCollisionObjectWrapper); - dmaPpuAddress2 = objectWrapper; - cellDmaGet(&lsMemPtr->gCollisionObjectWrapper, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - /* DMA Collision object into local store */ - dmaSize = sizeof(btCollisionObject); - dmaPpuAddress2 = lsMemPtr->getCollisionObjectWrapper()->getCollisionObjectPtr(); - cellDmaGet(&lsMemPtr->gColObj, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(2)); - - /* Gather information about collision object and shape */ - gatheredObjectData->m_worldTransform = lsMemPtr->getColObj()->getWorldTransform(); - gatheredObjectData->m_collisionMargin = lsMemPtr->getCollisionObjectWrapper()->getCollisionMargin (); - gatheredObjectData->m_shapeType = lsMemPtr->getCollisionObjectWrapper()->getShapeType (); - gatheredObjectData->m_collisionShape = (ppu_address_t)lsMemPtr->getColObj()->getCollisionShape(); - gatheredObjectData->m_spuCollisionShape = (void*)&lsMemPtr->gCollisionShape.collisionShape; - - /* DMA shape data */ - dmaCollisionShape (gatheredObjectData->m_spuCollisionShape, gatheredObjectData->m_collisionShape, 1, gatheredObjectData->m_shapeType); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - if (btBroadphaseProxy::isConvex (gatheredObjectData->m_shapeType)) - { - btConvexInternalShape* spuConvexShape = (btConvexInternalShape*)gatheredObjectData->m_spuCollisionShape; - gatheredObjectData->m_primitiveDimensions = spuConvexShape->getImplicitShapeDimensions (); - } else { - gatheredObjectData->m_primitiveDimensions = btVector3(1.0, 1.0, 1.0); - } - -} - -void dmaLoadRayOutput (ppu_address_t rayOutputAddr, SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag) -{ - cellDmaGet(rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0); -} - -void dmaStoreRayOutput (ppu_address_t rayOutputAddr, const SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag) -{ - cellDmaLargePut (rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0); -} - -#if 0 -SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size) -{ -#if USE_SOFTWARE_CACHE - // Check for alignment requirements. We need to make sure the entire request fits within one cache line, - // so the first and last bytes should fall on the same cache line - btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); - - void* ls = spe_cache_read(ea); - memcpy(buffer, ls, size); -#else - stallingUnalignedDmaSmallGet(buffer,ea,size); -#endif -} -#endif - -void small_cache_read_triple( void* ls0, ppu_address_t ea0, - void* ls1, ppu_address_t ea1, - void* ls2, ppu_address_t ea2, - size_t size) -{ - btAssert(size<16); - ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]); - ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]); - ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]); - - uint32_t i; - - - ///make sure last 4 bits are the same, for cellDmaSmallGet - char* localStore0 = (char*)ls0; - uint32_t last4BitsOffset = ea0 & 0x0f; - char* tmpTarget0 = tmpBuffer0 + last4BitsOffset; - tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0); - - - char* localStore1 = (char*)ls1; - last4BitsOffset = ea1 & 0x0f; - char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; - tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0); - - char* localStore2 = (char*)ls2; - last4BitsOffset = ea2 & 0x0f; - char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; - tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0); - - - cellDmaWaitTagStatusAll( DMA_MASK(1) ); - - //this is slowish, perhaps memcpy on SPU is smarter? - for (i=0; btLikely( ibvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) - { - short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - ATTRIBUTE_ALIGNED16(short int tmpIndices[3]); - - small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], - &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], - &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(short int)); - - m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); - m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); - m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); - } else - { - int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - - small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], - &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], - &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(int)); - } - - //printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]); - // spu_printf("SPU index0=%d ,",spuIndices[0]); - // spu_printf("SPU index1=%d ,",spuIndices[1]); - // spu_printf("SPU index2=%d ,",spuIndices[2]); - // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); - - const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); - - for (int j=2;btLikely( j>=0 );j--) - { - int graphicsindex = m_lsMemPtr->spuIndices[j]; - - //spu_printf("SPU index=%d ,",graphicsindex); - btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); - - // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); - - - ///handle un-aligned vertices... - - //another DMA for each vertex - small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], - &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], - &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], - sizeof(btScalar)); - - //printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]); - spuTriangleVertices[j] = btVector3( - spuUnscaledVertex[0]*meshScaling.getX(), - spuUnscaledVertex[1]*meshScaling.getY(), - spuUnscaledVertex[2]*meshScaling.getZ()); - - //spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); - } - - RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData); - triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; - triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0]; - - //printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]); - //printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]); - //printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]); - SpuRaycastTaskWorkUnitOut out; - out.hitFraction = 1.0; - performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[m_workUnit], &out, m_lsMemPtr); - /* XXX: For now only take the closest hit */ - if (out.hitFraction < m_workUnitsOut[m_workUnit].hitFraction) - { - m_workUnitsOut[m_workUnit].hitFraction = out.hitFraction; - m_workUnitsOut[m_workUnit].hitNormal = out.hitNormal; - } - } - -}; - -class spuRaycastNodeCallback : public btNodeOverlapCallback -{ - RaycastGatheredObjectData* m_gatheredObjectData; - const SpuRaycastTaskWorkUnit* m_workUnits; - SpuRaycastTaskWorkUnitOut* m_workUnitsOut; - int m_numWorkUnits; - RaycastTask_LocalStoreMemory* m_lsMemPtr; - - ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]); - ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]); - //ATTRIBUTE_ALIGNED16(int spuIndices[16]); -public: - spuRaycastNodeCallback(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr) - : m_gatheredObjectData(gatheredObjectData), - m_workUnits(workUnits), - m_workUnitsOut(workUnitsOut), - m_numWorkUnits(numWorkUnits), - m_lsMemPtr (lsMemPtr) - { - } - - virtual void processNode(int subPart, int triangleIndex) - { - ///Create a triangle on the stack, call process collision, with GJK - ///DMA the vertices, can benefit from software caching - - // spu_printf("processNode with triangleIndex %d\n",triangleIndex); - - // ugly solution to support both 16bit and 32bit indices - if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) - { - short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - ATTRIBUTE_ALIGNED16(short int tmpIndices[3]); - - small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], - &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], - &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(short int)); - - m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); - m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); - m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); - } else - { - int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); - - small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], - &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], - &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], - sizeof(int)); - } - - //printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]); - // spu_printf("SPU index0=%d ,",spuIndices[0]); - // spu_printf("SPU index1=%d ,",spuIndices[1]); - // spu_printf("SPU index2=%d ,",spuIndices[2]); - // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); - - const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); - - for (int j=2;btLikely( j>=0 );j--) - { - int graphicsindex = m_lsMemPtr->spuIndices[j]; - - //spu_printf("SPU index=%d ,",graphicsindex); - btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); - - // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); - - - ///handle un-aligned vertices... - - //another DMA for each vertex - small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], - &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], - &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], - sizeof(btScalar)); - - //printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]); - spuTriangleVertices[j] = btVector3( - spuUnscaledVertex[0]*meshScaling.getX(), - spuUnscaledVertex[1]*meshScaling.getY(), - spuUnscaledVertex[2]*meshScaling.getZ()); - - //spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); - } - - RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData); - triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; - triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0]; - - //printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]); - //printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]); - //printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]); - for (int i = 0; i < m_numWorkUnits; i++) - { - SpuRaycastTaskWorkUnitOut out; - out.hitFraction = 1.0; - performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[i], &out, m_lsMemPtr); - /* XXX: For now only take the closest hit */ - if (out.hitFraction < m_workUnitsOut[i].hitFraction) - { - m_workUnitsOut[i].hitFraction = out.hitFraction; - m_workUnitsOut[i].hitNormal = out.hitNormal; - } - } - } - -}; - - -void spuWalkStacklessQuantizedTreeAgainstRays(RaycastTask_LocalStoreMemory* lsMemPtr, - btNodeOverlapCallback* nodeCallback, - const btVector3* rayFrom, - const btVector3* rayTo, - int numWorkUnits, - unsigned short int* quantizedQueryAabbMin, - unsigned short int* quantizedQueryAabbMax, - const btQuantizedBvhNode* rootNode, - int startNodeIndex,int endNodeIndex) -{ - int curIndex = startNodeIndex; - int walkIterations = 0; - int subTreeSize = endNodeIndex - startNodeIndex; - - int escapeIndex; - - unsigned int boxBoxOverlap, rayBoxOverlap, anyRayBoxOverlap; - unsigned int isLeafNode; - -#define RAYAABB2 -#ifdef RAYAABB2 - unsigned int sign[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; - btVector3 rayInvDirection[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - btScalar lambda_max[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - for (int i = 0; i < numWorkUnits; i++) - { - btVector3 rayDirection = (rayTo[i]-rayFrom[i]); - rayDirection.normalize (); - lambda_max[i] = rayDirection.dot(rayTo[i]-rayFrom[i]); - rayInvDirection[i][0] = btScalar(1.0) / rayDirection[0]; - rayInvDirection[i][1] = btScalar(1.0) / rayDirection[1]; - rayInvDirection[i][2] = btScalar(1.0) / rayDirection[2]; - sign[i][0] = rayDirection[0] < 0.0; - sign[i][1] = rayDirection[1] < 0.0; - sign[i][2] = rayDirection[2] < 0.0; - } -#endif - - while (curIndex < endNodeIndex) - { - //catch bugs in tree data - assert (walkIterations < subTreeSize); - - walkIterations++; - - isLeafNode = rootNode->isLeafNode(); - - anyRayBoxOverlap = 0; - - for (int i = 0; i < numWorkUnits; i++) - { - unsigned short int* quamin = (quantizedQueryAabbMin + 3 * i); - unsigned short int* quamax = (quantizedQueryAabbMax + 3 * i); - boxBoxOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quamin,quamax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); - if (!boxBoxOverlap) - continue; - - rayBoxOverlap = 0; - btScalar param = 1.0; - btVector3 normal; - btVector3 bounds[2]; - bounds[0] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMin); - bounds[1] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMax); -#ifdef RAYAABB2 - rayBoxOverlap = btRayAabb2 (rayFrom[i], rayInvDirection[i], sign[i], bounds, param, 0.0, lambda_max[i]); -#else - rayBoxOverlap = btRayAabb(rayFrom[i], rayTo[i], bounds[0], bounds[1], param, normal); -#endif - -#ifndef CALLBACK_ALL - anyRayBoxOverlap = rayBoxOverlap || anyRayBoxOverlap; - /* If we have any ray vs. box overlap and this isn't a leaf node - we know that we need to dig deeper - */ - if (!isLeafNode && anyRayBoxOverlap) - break; - - if (isLeafNode && rayBoxOverlap) - { - spuRaycastNodeCallback1* callback = (spuRaycastNodeCallback1*)nodeCallback; - callback->setWorkUnit (i); - nodeCallback->processNode (0, rootNode->getTriangleIndex()); - } -#else - /* If we have any ray vs. box overlap and this isn't a leaf node - we know that we need to dig deeper - */ - if (rayBoxOverlap) - { - anyRayBoxOverlap = 1; - break; - } -#endif - } - -#ifdef CALLBACK_ALL - if (isLeafNode && anyRayBoxOverlap) - { - nodeCallback->processNode (0, rootNode->getTriangleIndex()); - } -#endif - - if (anyRayBoxOverlap || isLeafNode) - { - rootNode++; - curIndex++; - } else - { - escapeIndex = rootNode->getEscapeIndex(); - rootNode += escapeIndex; - curIndex += escapeIndex; - } - } - -} - - -void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr) -{ - //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - - - btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)gatheredObjectData->m_spuCollisionShape; - - //need the mesh interface, for access to triangle vertices - dmaBvhShapeData (&(lsMemPtr->bvhShapeData), trimeshShape); - - unsigned short int quantizedQueryAabbMin[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; - unsigned short int quantizedQueryAabbMax[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; - btVector3 rayFromInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - btVector3 rayToInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - - /* Calculate the AABB for the ray in the triangle mesh shape */ - btTransform rayInTriangleSpace; - rayInTriangleSpace = gatheredObjectData->m_worldTransform.inverse(); - - for (int i = 0; i < numWorkUnits; i++) - { - btVector3 aabbMin; - btVector3 aabbMax; - - rayFromInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayFrom); - rayToInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayTo); - - aabbMin = rayFromInTriangleSpace[i]; - aabbMin.setMin (rayToInTriangleSpace[i]); - aabbMax = rayFromInTriangleSpace[i]; - aabbMax.setMax (rayToInTriangleSpace[i]); - - lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin[i],aabbMin,0); - lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax[i],aabbMax,1); - } - - QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray(); - //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); - - BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray(); - -#ifdef CALLBACK_ALL - spuRaycastNodeCallback nodeCallback (gatheredObjectData, workUnits, workUnitsOut, numWorkUnits, lsMemPtr); -#else - spuRaycastNodeCallback1 nodeCallback (gatheredObjectData, workUnits, workUnitsOut, lsMemPtr); -#endif - - IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray(); - - //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); - // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); - //not likely to happen - if (subTrees.size() && indexArray.size() == 1) - { - ///DMA in the index info - dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - //display the headers - int numBatch = subTrees.size(); - for (int i=0;ibvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - // spu_printf("nextBatch = %d\n",nextBatch); - - - for (int j=0;jbvhShapeData.gSubtreeHeaders[j]; - - unsigned int overlap = 1; - for (int boxId = 0; boxId < numWorkUnits; boxId++) - { - overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin[boxId],quantizedQueryAabbMax[boxId],subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); - if (overlap) - break; - } - - if (overlap) - { - btAssert(subtree.m_subtreeSize); - - //dma the actual nodes of this subtree - dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2); - - cellDmaWaitTagStatusAll(DMA_MASK(2)); - - /* Walk this subtree */ - - { - - spuWalkStacklessQuantizedTreeAgainstRays(lsMemPtr, - &nodeCallback, - &rayFromInTriangleSpace[0], - &rayToInTriangleSpace[0], - numWorkUnits, - &quantizedQueryAabbMin[0][0],&quantizedQueryAabbMax[0][0], - &lsMemPtr->bvhShapeData.gSubtreeNodes[0], 0, subtree.m_subtreeSize); - } - } - // spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); - } - - // unsigned short int m_quantizedAabbMin[3]; - // unsigned short int m_quantizedAabbMax[3]; - // int m_rootNodeIndex; - // int m_subtreeSize; - i+=nextBatch; - } - - //pre-fetch first tree, then loop and double buffer - } - -} - -void performRaycastAgainstCompound (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr) -{ - //XXX spu_printf ("Currently no support for ray. vs compound objects. Support coming soon.\n"); -} - -void -performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr) -{ - SpuVoronoiSimplexSolver simplexSolver; - - btTransform rayFromTrans, rayToTrans; - rayFromTrans.setIdentity (); - rayFromTrans.setOrigin (workUnit.rayFrom); - rayToTrans.setIdentity (); - rayToTrans.setOrigin (workUnit.rayTo); - - SpuCastResult result; - - /* Load the vertex data if the shape is a convex hull */ - /* XXX: We might be loading the shape twice */ - ATTRIBUTE_ALIGNED16(char convexHullShape[sizeof(btConvexHullShape)]); - if (gatheredObjectData->m_shapeType == CONVEX_HULL_SHAPE_PROXYTYPE) - { - register int dmaSize; - register ppu_address_t dmaPpuAddress2; - dmaSize = sizeof(btConvexHullShape); - dmaPpuAddress2 = gatheredObjectData->m_collisionShape; - cellDmaGet(&convexHullShape, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - dmaConvexVertexData (&lsMemPtr->convexVertexData, (btConvexHullShape*)&convexHullShape); - cellDmaWaitTagStatusAll(DMA_MASK(2)); // dmaConvexVertexData uses dma channel 2! - lsMemPtr->convexVertexData.gSpuConvexShapePtr = gatheredObjectData->m_spuCollisionShape; - lsMemPtr->convexVertexData.gConvexPoints = &lsMemPtr->convexVertexData.g_convexPointBuffer[0]; - } - - /* performRaycast */ - SpuSubsimplexRayCast caster (gatheredObjectData->m_spuCollisionShape, &lsMemPtr->convexVertexData, gatheredObjectData->m_shapeType, gatheredObjectData->m_collisionMargin, &simplexSolver); - bool r = caster.calcTimeOfImpact (rayFromTrans, rayToTrans, gatheredObjectData->m_worldTransform, gatheredObjectData->m_worldTransform,result); - - if (r) - { - workUnitOut->hitFraction = result.m_fraction; - workUnitOut->hitNormal = result.m_normal; - } -} - -void processRaycastTask(void* userPtr, void* lsMemory) -{ - RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory; - - SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr; - SpuRaycastTaskDesc& taskDesc = *taskDescPtr; - - SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers; - - //spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers); - /* for each object */ - RaycastGatheredObjectData gatheredObjectData; - for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++) - { - //spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers); - - /* load initial collision shape */ - GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]); - - if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType)) - { - SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) - { - tWorkUnitsOut[rayId].hitFraction = 1.0; - } - - performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory); - - for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) - { - const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; - if (tWorkUnitsOut[rayId].hitFraction == 1.0) - continue; - - ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); - dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - /* XXX Only support taking the closest hit for now */ - if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction) - { - workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction; - workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal; - } - - /* write ray cast data back */ - dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - } else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) { - - btVector3 objectBoxMin, objectBoxMax; - computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform); - for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) - { - const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; - - btScalar ignored_param = 1.0; - btVector3 ignored_normal; - if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal)) - { - ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); - SpuRaycastTaskWorkUnitOut tWorkUnitOut; - tWorkUnitOut.hitFraction = 1.0; - - performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory); - if (tWorkUnitOut.hitFraction == 1.0) - continue; - - dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - /* XXX Only support taking the closest hit for now */ - if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction) - { - workUnitOut.hitFraction = tWorkUnitOut.hitFraction; - workUnitOut.hitNormal = tWorkUnitOut.hitNormal; - /* write ray cast data back */ - dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - } - } - - } else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) { - for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) - { - const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; - ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); - SpuRaycastTaskWorkUnitOut tWorkUnitOut; - tWorkUnitOut.hitFraction = 1.0; - - performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory); - if (tWorkUnitOut.hitFraction == 1.0) - continue; - - dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - /* XXX Only support taking the closest hit for now */ - if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction) - { - workUnitOut.hitFraction = tWorkUnitOut.hitFraction; - workUnitOut.hitNormal = tWorkUnitOut.hitNormal; - } - - /* write ray cast data back */ - dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - } - } -} + + +#include "../PlatformDefinitions.h" +#include "SpuRaycastTask.h" +#include "../SpuCollisionObjectWrapper.h" +#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h" +#include "SpuSubSimplexConvexCast.h" +#include "LinearMath/btAabbUtil2.h" + + +/* Future optimization strategies: +1. BBOX prune before loading shape data +2. Could reduce number of dmas for ray output data to a single read and write. + By sharing the temporary work unit output structures across objects. +3. The reason SpuRaycastNodeCallback1 is slower is because the triangle data isn't + being cached across calls. Fix that by doing the final ray pruning inside the callback. +*/ + +/* Future work: +1. support first hit, closest hit, etc rather than just closest hit. +2. support compound objects +*/ + +#define CALLBACK_ALL + +struct RaycastTask_LocalStoreMemory +{ + ATTRIBUTE_ALIGNED16(char gColObj [sizeof(btCollisionObject)+16]); + btCollisionObject* getColObj() + { + return (btCollisionObject*) gColObj; + } + + ATTRIBUTE_ALIGNED16(SpuCollisionObjectWrapper gCollisionObjectWrapper); + SpuCollisionObjectWrapper* getCollisionObjectWrapper () + { + return &gCollisionObjectWrapper; + } + + CollisionShape_LocalStoreMemory gCollisionShape; + ATTRIBUTE_ALIGNED16(int spuIndices[16]); + + bvhMeshShape_LocalStoreMemory bvhShapeData; + SpuConvexPolyhedronVertexData convexVertexData; + CompoundShape_LocalStoreMemory compoundShapeData; +}; + +#ifdef WIN32 +void* createRaycastLocalStoreMemory() +{ + return new RaycastTask_LocalStoreMemory; +}; +#elif defined(__CELLOS_LV2__) +ATTRIBUTE_ALIGNED16(RaycastTask_LocalStoreMemory gLocalStoreMemory); +void* createRaycastLocalStoreMemory() +{ + return &gLocalStoreMemory; +} +#endif + +void GatherCollisionObjectAndShapeData (RaycastGatheredObjectData* gatheredObjectData, RaycastTask_LocalStoreMemory* lsMemPtr, ppu_address_t objectWrapper) +{ + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + /* DMA Collision object wrapper into local store */ + dmaSize = sizeof(SpuCollisionObjectWrapper); + dmaPpuAddress2 = objectWrapper; + cellDmaGet(&lsMemPtr->gCollisionObjectWrapper, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + /* DMA Collision object into local store */ + dmaSize = sizeof(btCollisionObject); + dmaPpuAddress2 = lsMemPtr->getCollisionObjectWrapper()->getCollisionObjectPtr(); + cellDmaGet(&lsMemPtr->gColObj, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + /* Gather information about collision object and shape */ + gatheredObjectData->m_worldTransform = lsMemPtr->getColObj()->getWorldTransform(); + gatheredObjectData->m_collisionMargin = lsMemPtr->getCollisionObjectWrapper()->getCollisionMargin (); + gatheredObjectData->m_shapeType = lsMemPtr->getCollisionObjectWrapper()->getShapeType (); + gatheredObjectData->m_collisionShape = (ppu_address_t)lsMemPtr->getColObj()->getCollisionShape(); + gatheredObjectData->m_spuCollisionShape = (void*)&lsMemPtr->gCollisionShape.collisionShape; + + /* DMA shape data */ + dmaCollisionShape (gatheredObjectData->m_spuCollisionShape, gatheredObjectData->m_collisionShape, 1, gatheredObjectData->m_shapeType); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + if (btBroadphaseProxy::isConvex (gatheredObjectData->m_shapeType)) + { + btConvexInternalShape* spuConvexShape = (btConvexInternalShape*)gatheredObjectData->m_spuCollisionShape; + gatheredObjectData->m_primitiveDimensions = spuConvexShape->getImplicitShapeDimensions (); + } else { + gatheredObjectData->m_primitiveDimensions = btVector3(1.0, 1.0, 1.0); + } + +} + +void dmaLoadRayOutput (ppu_address_t rayOutputAddr, SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag) +{ + cellDmaGet(rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0); +} + +void dmaStoreRayOutput (ppu_address_t rayOutputAddr, const SpuRaycastTaskWorkUnitOut* rayOutput, uint32_t dmaTag) +{ + cellDmaLargePut (rayOutput, rayOutputAddr, sizeof(*rayOutput), DMA_TAG(dmaTag), 0, 0); +} + +#if 0 +SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size) +{ +#if USE_SOFTWARE_CACHE + // Check for alignment requirements. We need to make sure the entire request fits within one cache line, + // so the first and last bytes should fall on the same cache line + btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK)); + + void* ls = spe_cache_read(ea); + memcpy(buffer, ls, size); +#else + stallingUnalignedDmaSmallGet(buffer,ea,size); +#endif +} +#endif + +void small_cache_read_triple( void* ls0, ppu_address_t ea0, + void* ls1, ppu_address_t ea1, + void* ls2, ppu_address_t ea2, + size_t size) +{ + btAssert(size<16); + ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]); + ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]); + + uint32_t i; + + + ///make sure last 4 bits are the same, for cellDmaSmallGet + char* localStore0 = (char*)ls0; + uint32_t last4BitsOffset = ea0 & 0x0f; + char* tmpTarget0 = tmpBuffer0 + last4BitsOffset; + tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0); + + + char* localStore1 = (char*)ls1; + last4BitsOffset = ea1 & 0x0f; + char* tmpTarget1 = tmpBuffer1 + last4BitsOffset; + tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0); + + char* localStore2 = (char*)ls2; + last4BitsOffset = ea2 & 0x0f; + char* tmpTarget2 = tmpBuffer2 + last4BitsOffset; + tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0); + + + cellDmaWaitTagStatusAll( DMA_MASK(1) ); + + //this is slowish, perhaps memcpy on SPU is smarter? + for (i=0; btLikely( ibvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) + { + short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + ATTRIBUTE_ALIGNED16(short int tmpIndices[3]); + + small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], + &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], + &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(short int)); + + m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); + m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); + m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); + } else + { + int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + + small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], + &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], + &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(int)); + } + + //printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]); + // spu_printf("SPU index0=%d ,",spuIndices[0]); + // spu_printf("SPU index1=%d ,",spuIndices[1]); + // spu_printf("SPU index2=%d ,",spuIndices[2]); + // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); + + const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); + + for (int j=2;btLikely( j>=0 );j--) + { + int graphicsindex = m_lsMemPtr->spuIndices[j]; + + //spu_printf("SPU index=%d ,",graphicsindex); + btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); + + // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); + + + ///handle un-aligned vertices... + + //another DMA for each vertex + small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], + &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], + &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], + sizeof(btScalar)); + + //printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]); + spuTriangleVertices[j] = btVector3( + spuUnscaledVertex[0]*meshScaling.getX(), + spuUnscaledVertex[1]*meshScaling.getY(), + spuUnscaledVertex[2]*meshScaling.getZ()); + + //spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); + } + + RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData); + triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; + triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0]; + + //printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]); + //printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]); + //printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]); + SpuRaycastTaskWorkUnitOut out; + out.hitFraction = 1.0; + performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[m_workUnit], &out, m_lsMemPtr); + /* XXX: For now only take the closest hit */ + if (out.hitFraction < m_workUnitsOut[m_workUnit].hitFraction) + { + m_workUnitsOut[m_workUnit].hitFraction = out.hitFraction; + m_workUnitsOut[m_workUnit].hitNormal = out.hitNormal; + } + } + +}; + +class spuRaycastNodeCallback : public btNodeOverlapCallback +{ + RaycastGatheredObjectData* m_gatheredObjectData; + const SpuRaycastTaskWorkUnit* m_workUnits; + SpuRaycastTaskWorkUnitOut* m_workUnitsOut; + int m_numWorkUnits; + RaycastTask_LocalStoreMemory* m_lsMemPtr; + + ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]); + ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]); + //ATTRIBUTE_ALIGNED16(int spuIndices[16]); +public: + spuRaycastNodeCallback(RaycastGatheredObjectData* gatheredObjectData,const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr) + : m_gatheredObjectData(gatheredObjectData), + m_workUnits(workUnits), + m_workUnitsOut(workUnitsOut), + m_numWorkUnits(numWorkUnits), + m_lsMemPtr (lsMemPtr) + { + } + + virtual void processNode(int subPart, int triangleIndex) + { + ///Create a triangle on the stack, call process collision, with GJK + ///DMA the vertices, can benefit from software caching + + // spu_printf("processNode with triangleIndex %d\n",triangleIndex); + + // ugly solution to support both 16bit and 32bit indices + if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT) + { + short int* indexBasePtr = (short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + ATTRIBUTE_ALIGNED16(short int tmpIndices[3]); + + small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0], + &tmpIndices[1],(ppu_address_t)&indexBasePtr[1], + &tmpIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(short int)); + + m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]); + m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]); + m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]); + } else + { + int* indexBasePtr = (int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride); + + small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0], + &m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1], + &m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2], + sizeof(int)); + } + + //printf("%d %d %d\n", m_lsMemPtr->spuIndices[0], m_lsMemPtr->spuIndices[1], m_lsMemPtr->spuIndices[2]); + // spu_printf("SPU index0=%d ,",spuIndices[0]); + // spu_printf("SPU index1=%d ,",spuIndices[1]); + // spu_printf("SPU index2=%d ,",spuIndices[2]); + // spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr); + + const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling(); + + for (int j=2;btLikely( j>=0 );j--) + { + int graphicsindex = m_lsMemPtr->spuIndices[j]; + + //spu_printf("SPU index=%d ,",graphicsindex); + btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride); + + // spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr); + + + ///handle un-aligned vertices... + + //another DMA for each vertex + small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0], + &spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1], + &spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2], + sizeof(btScalar)); + + //printf("%f %f %f\n", spuUnscaledVertex[0],spuUnscaledVertex[1],spuUnscaledVertex[2]); + spuTriangleVertices[j] = btVector3( + spuUnscaledVertex[0]*meshScaling.getX(), + spuUnscaledVertex[1]*meshScaling.getY(), + spuUnscaledVertex[2]*meshScaling.getZ()); + + //spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z()); + } + + RaycastGatheredObjectData triangleGatheredObjectData (*m_gatheredObjectData); + triangleGatheredObjectData.m_shapeType = TRIANGLE_SHAPE_PROXYTYPE; + triangleGatheredObjectData.m_spuCollisionShape = &spuTriangleVertices[0]; + + //printf("%f %f %f\n", spuTriangleVertices[0][0],spuTriangleVertices[0][1],spuTriangleVertices[0][2]); + //printf("%f %f %f\n", spuTriangleVertices[1][0],spuTriangleVertices[1][1],spuTriangleVertices[1][2]); + //printf("%f %f %f\n", spuTriangleVertices[2][0],spuTriangleVertices[2][1],spuTriangleVertices[2][2]); + for (int i = 0; i < m_numWorkUnits; i++) + { + SpuRaycastTaskWorkUnitOut out; + out.hitFraction = 1.0; + performRaycastAgainstConvex (&triangleGatheredObjectData, m_workUnits[i], &out, m_lsMemPtr); + /* XXX: For now only take the closest hit */ + if (out.hitFraction < m_workUnitsOut[i].hitFraction) + { + m_workUnitsOut[i].hitFraction = out.hitFraction; + m_workUnitsOut[i].hitNormal = out.hitNormal; + } + } + } + +}; + + +void spuWalkStacklessQuantizedTreeAgainstRays(RaycastTask_LocalStoreMemory* lsMemPtr, + btNodeOverlapCallback* nodeCallback, + const btVector3* rayFrom, + const btVector3* rayTo, + int numWorkUnits, + unsigned short int* quantizedQueryAabbMin, + unsigned short int* quantizedQueryAabbMax, + const btQuantizedBvhNode* rootNode, + int startNodeIndex,int endNodeIndex) +{ + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + + int escapeIndex; + + unsigned int boxBoxOverlap, rayBoxOverlap, anyRayBoxOverlap; + unsigned int isLeafNode; + +#define RAYAABB2 +#ifdef RAYAABB2 + unsigned int sign[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; + btVector3 rayInvDirection[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + btScalar lambda_max[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + for (int i = 0; i < numWorkUnits; i++) + { + btVector3 rayDirection = (rayTo[i]-rayFrom[i]); + rayDirection.normalize (); + lambda_max[i] = rayDirection.dot(rayTo[i]-rayFrom[i]); + rayInvDirection[i][0] = btScalar(1.0) / rayDirection[0]; + rayInvDirection[i][1] = btScalar(1.0) / rayDirection[1]; + rayInvDirection[i][2] = btScalar(1.0) / rayDirection[2]; + sign[i][0] = rayDirection[0] < 0.0; + sign[i][1] = rayDirection[1] < 0.0; + sign[i][2] = rayDirection[2] < 0.0; + } +#endif + + while (curIndex < endNodeIndex) + { + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + walkIterations++; + + isLeafNode = rootNode->isLeafNode(); + + anyRayBoxOverlap = 0; + + for (int i = 0; i < numWorkUnits; i++) + { + unsigned short int* quamin = (quantizedQueryAabbMin + 3 * i); + unsigned short int* quamax = (quantizedQueryAabbMax + 3 * i); + boxBoxOverlap = spuTestQuantizedAabbAgainstQuantizedAabb(quamin,quamax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + if (!boxBoxOverlap) + continue; + + rayBoxOverlap = 0; + btScalar param = 1.0; + btVector3 normal; + btVector3 bounds[2]; + bounds[0] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = lsMemPtr->bvhShapeData.getOptimizedBvh()->unQuantize(rootNode->m_quantizedAabbMax); +#ifdef RAYAABB2 + rayBoxOverlap = btRayAabb2 (rayFrom[i], rayInvDirection[i], sign[i], bounds, param, 0.0, lambda_max[i]); +#else + rayBoxOverlap = btRayAabb(rayFrom[i], rayTo[i], bounds[0], bounds[1], param, normal); +#endif + +#ifndef CALLBACK_ALL + anyRayBoxOverlap = rayBoxOverlap || anyRayBoxOverlap; + /* If we have any ray vs. box overlap and this isn't a leaf node + we know that we need to dig deeper + */ + if (!isLeafNode && anyRayBoxOverlap) + break; + + if (isLeafNode && rayBoxOverlap) + { + spuRaycastNodeCallback1* callback = (spuRaycastNodeCallback1*)nodeCallback; + callback->setWorkUnit (i); + nodeCallback->processNode (0, rootNode->getTriangleIndex()); + } +#else + /* If we have any ray vs. box overlap and this isn't a leaf node + we know that we need to dig deeper + */ + if (rayBoxOverlap) + { + anyRayBoxOverlap = 1; + break; + } +#endif + } + +#ifdef CALLBACK_ALL + if (isLeafNode && anyRayBoxOverlap) + { + nodeCallback->processNode (0, rootNode->getTriangleIndex()); + } +#endif + + if (anyRayBoxOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + +} + + +void performRaycastAgainstConcave (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit* workUnits, SpuRaycastTaskWorkUnitOut* workUnitsOut, int numWorkUnits, RaycastTask_LocalStoreMemory* lsMemPtr) +{ + //order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + + + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)gatheredObjectData->m_spuCollisionShape; + + //need the mesh interface, for access to triangle vertices + dmaBvhShapeData (&(lsMemPtr->bvhShapeData), trimeshShape); + + unsigned short int quantizedQueryAabbMin[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; + unsigned short int quantizedQueryAabbMax[SPU_RAYCAST_WORK_UNITS_PER_TASK][3]; + btVector3 rayFromInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + btVector3 rayToInTriangleSpace[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + + /* Calculate the AABB for the ray in the triangle mesh shape */ + btTransform rayInTriangleSpace; + rayInTriangleSpace = gatheredObjectData->m_worldTransform.inverse(); + + for (int i = 0; i < numWorkUnits; i++) + { + btVector3 aabbMin; + btVector3 aabbMax; + + rayFromInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayFrom); + rayToInTriangleSpace[i] = rayInTriangleSpace(workUnits[i].rayTo); + + aabbMin = rayFromInTriangleSpace[i]; + aabbMin.setMin (rayToInTriangleSpace[i]); + aabbMax = rayFromInTriangleSpace[i]; + aabbMax.setMax (rayToInTriangleSpace[i]); + + lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin[i],aabbMin,0); + lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax[i],aabbMax,1); + } + + QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray(); + //spu_printf("SPU: numNodes = %d\n",nodeArray.size()); + + BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray(); + +#ifdef CALLBACK_ALL + spuRaycastNodeCallback nodeCallback (gatheredObjectData, workUnits, workUnitsOut, numWorkUnits, lsMemPtr); +#else + spuRaycastNodeCallback1 nodeCallback (gatheredObjectData, workUnits, workUnitsOut, lsMemPtr); +#endif + + IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray(); + + //spu_printf("SPU:indexArray.size() = %d\n",indexArray.size()); + // spu_printf("SPU: numSubTrees = %d\n",subTrees.size()); + //not likely to happen + if (subTrees.size() && indexArray.size() == 1) + { + ///DMA in the index info + dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + //display the headers + int numBatch = subTrees.size(); + for (int i=0;ibvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + // spu_printf("nextBatch = %d\n",nextBatch); + + + for (int j=0;jbvhShapeData.gSubtreeHeaders[j]; + + unsigned int overlap = 1; + for (int boxId = 0; boxId < numWorkUnits; boxId++) + { + overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin[boxId],quantizedQueryAabbMax[boxId],subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap) + break; + } + + if (overlap) + { + btAssert(subtree.m_subtreeSize); + + //dma the actual nodes of this subtree + dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2); + + cellDmaWaitTagStatusAll(DMA_MASK(2)); + + /* Walk this subtree */ + + { + + spuWalkStacklessQuantizedTreeAgainstRays(lsMemPtr, + &nodeCallback, + &rayFromInTriangleSpace[0], + &rayToInTriangleSpace[0], + numWorkUnits, + &quantizedQueryAabbMin[0][0],&quantizedQueryAabbMax[0][0], + &lsMemPtr->bvhShapeData.gSubtreeNodes[0], 0, subtree.m_subtreeSize); + } + } + // spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize); + } + + // unsigned short int m_quantizedAabbMin[3]; + // unsigned short int m_quantizedAabbMax[3]; + // int m_rootNodeIndex; + // int m_subtreeSize; + i+=nextBatch; + } + + //pre-fetch first tree, then loop and double buffer + } + +} + +void performRaycastAgainstCompound (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr) +{ + //XXX spu_printf ("Currently no support for ray. vs compound objects. Support coming soon.\n"); +} + +void +performRaycastAgainstConvex (RaycastGatheredObjectData* gatheredObjectData, const SpuRaycastTaskWorkUnit& workUnit, SpuRaycastTaskWorkUnitOut* workUnitOut, RaycastTask_LocalStoreMemory* lsMemPtr) +{ + SpuVoronoiSimplexSolver simplexSolver; + + btTransform rayFromTrans, rayToTrans; + rayFromTrans.setIdentity (); + rayFromTrans.setOrigin (workUnit.rayFrom); + rayToTrans.setIdentity (); + rayToTrans.setOrigin (workUnit.rayTo); + + SpuCastResult result; + + /* Load the vertex data if the shape is a convex hull */ + /* XXX: We might be loading the shape twice */ + ATTRIBUTE_ALIGNED16(char convexHullShape[sizeof(btConvexHullShape)]); + if (gatheredObjectData->m_shapeType == CONVEX_HULL_SHAPE_PROXYTYPE) + { + register int dmaSize; + register ppu_address_t dmaPpuAddress2; + dmaSize = sizeof(btConvexHullShape); + dmaPpuAddress2 = gatheredObjectData->m_collisionShape; + cellDmaGet(&convexHullShape, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + dmaConvexVertexData (&lsMemPtr->convexVertexData, (btConvexHullShape*)&convexHullShape); + cellDmaWaitTagStatusAll(DMA_MASK(2)); // dmaConvexVertexData uses dma channel 2! + lsMemPtr->convexVertexData.gSpuConvexShapePtr = gatheredObjectData->m_spuCollisionShape; + lsMemPtr->convexVertexData.gConvexPoints = &lsMemPtr->convexVertexData.g_convexPointBuffer[0]; + } + + /* performRaycast */ + SpuSubsimplexRayCast caster (gatheredObjectData->m_spuCollisionShape, &lsMemPtr->convexVertexData, gatheredObjectData->m_shapeType, gatheredObjectData->m_collisionMargin, &simplexSolver); + bool r = caster.calcTimeOfImpact (rayFromTrans, rayToTrans, gatheredObjectData->m_worldTransform, gatheredObjectData->m_worldTransform,result); + + if (r) + { + workUnitOut->hitFraction = result.m_fraction; + workUnitOut->hitNormal = result.m_normal; + } +} + +void processRaycastTask(void* userPtr, void* lsMemory) +{ + RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory; + + SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr; + SpuRaycastTaskDesc& taskDesc = *taskDescPtr; + + SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers; + + //spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers); + /* for each object */ + RaycastGatheredObjectData gatheredObjectData; + for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++) + { + //spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers); + + /* load initial collision shape */ + GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]); + + if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType)) + { + SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) + { + tWorkUnitsOut[rayId].hitFraction = 1.0; + } + + performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory); + + for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) + { + const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; + if (tWorkUnitsOut[rayId].hitFraction == 1.0) + continue; + + ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); + dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + /* XXX Only support taking the closest hit for now */ + if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction) + { + workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction; + workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal; + } + + /* write ray cast data back */ + dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + } else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) { + + btVector3 objectBoxMin, objectBoxMax; + computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform); + for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) + { + const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; + + btScalar ignored_param = 1.0; + btVector3 ignored_normal; + if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal)) + { + ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); + SpuRaycastTaskWorkUnitOut tWorkUnitOut; + tWorkUnitOut.hitFraction = 1.0; + + performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory); + if (tWorkUnitOut.hitFraction == 1.0) + continue; + + dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + /* XXX Only support taking the closest hit for now */ + if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction) + { + workUnitOut.hitFraction = tWorkUnitOut.hitFraction; + workUnitOut.hitNormal = tWorkUnitOut.hitNormal; + /* write ray cast data back */ + dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + } + } + + } else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) { + for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++) + { + const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId]; + ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut); + SpuRaycastTaskWorkUnitOut tWorkUnitOut; + tWorkUnitOut.hitFraction = 1.0; + + performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory); + if (tWorkUnitOut.hitFraction == 1.0) + continue; + + dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + /* XXX Only support taking the closest hit for now */ + if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction) + { + workUnitOut.hitFraction = tWorkUnitOut.hitFraction; + workUnitOut.hitNormal = tWorkUnitOut.hitNormal; + } + + /* write ray cast data back */ + dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + } + } +} diff --git a/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h b/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h index a8312eda6..004195167 100644 --- a/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h +++ b/src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h @@ -1,50 +1,50 @@ -#ifndef __SPU_RAYCAST_TASK_H -#define __SPU_RAYCAST_TASK_H - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "LinearMath/btVector3.h" -#include "../PlatformDefinitions.h" - -ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData -{ - ppu_address_t m_collisionShape; - void* m_spuCollisionShape; - btVector3 m_primitiveDimensions; - int m_shapeType; - float m_collisionMargin; - btTransform m_worldTransform; -}; - - -ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut -{ - btVector3 hitNormal; /* out */ - btScalar hitFraction; /* out */ - btCollisionWorld::LocalShapeInfo shapeInfo; /* out */ -}; - -/* Perform a raycast on collision object */ -ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit -{ - btVector3 rayFrom; /* in */ - btVector3 rayTo; /* in */ - SpuRaycastTaskWorkUnitOut* output; /* out */ -}; - -#define SPU_RAYCAST_WORK_UNITS_PER_TASK 16 - -ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc -{ - SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK]; - unsigned int numWorkUnits; - void* spuCollisionObjectsWrappers; - unsigned int numSpuCollisionObjectWrappers; - int taskId; -}; - - -void processRaycastTask (void* userPtr, void* lsMemory); -void* createRaycastLocalStoreMemory (); - -#endif +#ifndef __SPU_RAYCAST_TASK_H +#define __SPU_RAYCAST_TASK_H + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "LinearMath/btVector3.h" +#include "../PlatformDefinitions.h" + +ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData +{ + ppu_address_t m_collisionShape; + void* m_spuCollisionShape; + btVector3 m_primitiveDimensions; + int m_shapeType; + float m_collisionMargin; + btTransform m_worldTransform; +}; + + +ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut +{ + btVector3 hitNormal; /* out */ + btScalar hitFraction; /* out */ + btCollisionWorld::LocalShapeInfo shapeInfo; /* out */ +}; + +/* Perform a raycast on collision object */ +ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit +{ + btVector3 rayFrom; /* in */ + btVector3 rayTo; /* in */ + SpuRaycastTaskWorkUnitOut* output; /* out */ +}; + +#define SPU_RAYCAST_WORK_UNITS_PER_TASK 16 + +ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc +{ + SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK]; + unsigned int numWorkUnits; + void* spuCollisionObjectsWrappers; + unsigned int numSpuCollisionObjectWrappers; + int taskId; +}; + + +void processRaycastTask (void* userPtr, void* lsMemory); +void* createRaycastLocalStoreMemory (); + +#endif diff --git a/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp b/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp index dacf7e16a..6f6d43d23 100644 --- a/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp +++ b/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp @@ -1,152 +1,152 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuSubSimplexConvexCast.h" - - -#include "BulletCollision/CollisionShapes/btConvexShape.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" - - -SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, - SpuVoronoiSimplexSolver* simplexSolver) - :m_simplexSolver(simplexSolver), m_shapeB(shapeB), m_convexDataB(convexDataB), m_shapeTypeB(shapeTypeB), m_marginB(marginB) -{ -} - -///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. -///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 -#ifdef BT_USE_DOUBLE_PRECISION -#define MAX_ITERATIONS 64 -#else -#define MAX_ITERATIONS 32 -#endif - -/* Returns the support point of the minkowski sum: - * MSUM(Pellet, ConvexShape) - * - */ -void supportPoints (const btTransform& xformRay, - const btTransform& xformB, - const int shapeType, - const void* shape, - SpuConvexPolyhedronVertexData* convexVertexData, - const btScalar marginB, - const btVector3& seperatingAxis, - btVector3& w, - btVector3& supVertexRay, - btVector3& supVertexB) -{ - btVector3 saUnit = seperatingAxis; - saUnit.normalize(); - btVector3 SupportPellet = xformRay(0.0001 * -saUnit); - btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis(); - btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData)); - SupportShape += saUnit * marginB; - w = SupportPellet - SupportShape; - supVertexRay = SupportPellet; - supVertexB = SupportShape; -} - -bool SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay, - const btTransform& toRay, - const btTransform& fromB, - const btTransform& toB, - SpuCastResult& result) -{ - m_simplexSolver->reset(); - - btVector3 linVelRay, linVelB; - linVelRay = toRay.getOrigin() - fromRay.getOrigin(); - linVelB = toB.getOrigin() - fromB.getOrigin (); - - btScalar lambda = btScalar(0.); - - btTransform interpolatedTransRay = fromRay; - btTransform interpolatedTransB = fromB; - - btVector3 r = (linVelRay-linVelB); - btVector3 supVertexRay; - btVector3 supVertexB; - btVector3 v; - supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB); - - btVector3 n; - n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); - bool hasResult = false; - btVector3 c; - int maxIter = MAX_ITERATIONS; - - btScalar lastLambda = lambda; - - btScalar dist2 = v.length2(); - -#ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); -#else - btScalar epsilon = btScalar(0.0001); -#endif //BT_USE_DOUBLE_PRECISION - btVector3 w,p; - btScalar VdotR; - - while ( (dist2 > epsilon) && maxIter--) - { - supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB); - - btScalar VdotW = v.dot(w); - - if (lambda > btScalar(1.0)) - { - return false; - } - - if ( VdotW > btScalar(0.)) - { - VdotR = v.dot(r); - - if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) - return false; - else - { - lambda = lambda - VdotW / VdotR; - interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda); - interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); - lastLambda = lambda; - n = v; - hasResult = true; - } - } - m_simplexSolver->addVertex(w, supVertexRay, supVertexB); - if (m_simplexSolver->closest(v)) - { - dist2 = v.length2(); - hasResult = true; - //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); - //printf("DIST2=%f\n",dist2); - //printf("numverts = %i\n",m_simplexSolver->numVertices()); - } else - { - dist2 = btScalar(0.); - } - } - - result.m_fraction = lambda; - result.m_normal = n; - btVector3 hitRay, hitB; - m_simplexSolver->compute_points (hitRay, hitB); - /* TODO: We could output hit point here (hitB) */ - return true; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuSubSimplexConvexCast.h" + + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" + + +SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, + SpuVoronoiSimplexSolver* simplexSolver) + :m_simplexSolver(simplexSolver), m_shapeB(shapeB), m_convexDataB(convexDataB), m_shapeTypeB(shapeTypeB), m_marginB(marginB) +{ +} + +///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. +///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 +#ifdef BT_USE_DOUBLE_PRECISION +#define MAX_ITERATIONS 64 +#else +#define MAX_ITERATIONS 32 +#endif + +/* Returns the support point of the minkowski sum: + * MSUM(Pellet, ConvexShape) + * + */ +void supportPoints (const btTransform& xformRay, + const btTransform& xformB, + const int shapeType, + const void* shape, + SpuConvexPolyhedronVertexData* convexVertexData, + const btScalar marginB, + const btVector3& seperatingAxis, + btVector3& w, + btVector3& supVertexRay, + btVector3& supVertexB) +{ + btVector3 saUnit = seperatingAxis; + saUnit.normalize(); + btVector3 SupportPellet = xformRay(0.0001 * -saUnit); + btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis(); + btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData)); + SupportShape += saUnit * marginB; + w = SupportPellet - SupportShape; + supVertexRay = SupportPellet; + supVertexB = SupportShape; +} + +bool SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay, + const btTransform& toRay, + const btTransform& fromB, + const btTransform& toB, + SpuCastResult& result) +{ + m_simplexSolver->reset(); + + btVector3 linVelRay, linVelB; + linVelRay = toRay.getOrigin() - fromRay.getOrigin(); + linVelB = toB.getOrigin() - fromB.getOrigin (); + + btScalar lambda = btScalar(0.); + + btTransform interpolatedTransRay = fromRay; + btTransform interpolatedTransB = fromB; + + btVector3 r = (linVelRay-linVelB); + btVector3 supVertexRay; + btVector3 supVertexB; + btVector3 v; + supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB); + + btVector3 n; + n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + bool hasResult = false; + btVector3 c; + int maxIter = MAX_ITERATIONS; + + btScalar lastLambda = lambda; + + btScalar dist2 = v.length2(); + +#ifdef BT_USE_DOUBLE_PRECISION + btScalar epsilon = btScalar(0.0001); +#else + btScalar epsilon = btScalar(0.0001); +#endif //BT_USE_DOUBLE_PRECISION + btVector3 w,p; + btScalar VdotR; + + while ( (dist2 > epsilon) && maxIter--) + { + supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB); + + btScalar VdotW = v.dot(w); + + if (lambda > btScalar(1.0)) + { + return false; + } + + if ( VdotW > btScalar(0.)) + { + VdotR = v.dot(r); + + if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) + return false; + else + { + lambda = lambda - VdotW / VdotR; + interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda); + interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); + lastLambda = lambda; + n = v; + hasResult = true; + } + } + m_simplexSolver->addVertex(w, supVertexRay, supVertexB); + if (m_simplexSolver->closest(v)) + { + dist2 = v.length2(); + hasResult = true; + //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); + //printf("DIST2=%f\n",dist2); + //printf("numverts = %i\n",m_simplexSolver->numVertices()); + } else + { + dist2 = btScalar(0.); + } + } + + result.m_fraction = lambda; + result.m_normal = n; + btVector3 hitRay, hitB; + m_simplexSolver->compute_points (hitRay, hitB); + /* TODO: We could output hit point here (hitB) */ + return true; +} diff --git a/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h b/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h index 23b6899b4..23901007c 100644 --- a/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h +++ b/src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h @@ -1,60 +1,60 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef SPU_SUBSIMPLEX_RAY_CAST_H -#define SPU_SUBSIMPLEX_RAY_CAST_H - -#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h" -#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h" -#include "SpuRaycastTask.h" - -class btConvexShape; - -struct SpuCastResult -{ - float m_fraction; - btVector3 m_normal; -}; - -/// btSubsimplexConvexCast implements Gino van den Bergens' paper -///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" -/// GJK based Ray Cast, optimized version -/// Objects should not start in overlap, otherwise results are not defined. -class SpuSubsimplexRayCast -{ - SpuVoronoiSimplexSolver* m_simplexSolver; - void* m_shapeB; - SpuConvexPolyhedronVertexData* m_convexDataB; - int m_shapeTypeB; - float m_marginB; - -public: - SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, - SpuVoronoiSimplexSolver* simplexSolver); - - //virtual ~btSubsimplexConvexCast(); - - ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. - ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector. - bool calcTimeOfImpact(const btTransform& fromRay, - const btTransform& toRay, - const btTransform& fromB, - const btTransform& toB, - SpuCastResult& result); - -}; - -#endif //SUBSIMPLEX_RAY_CAST_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef SPU_SUBSIMPLEX_RAY_CAST_H +#define SPU_SUBSIMPLEX_RAY_CAST_H + +#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h" +#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h" +#include "SpuRaycastTask.h" + +class btConvexShape; + +struct SpuCastResult +{ + float m_fraction; + btVector3 m_normal; +}; + +/// btSubsimplexConvexCast implements Gino van den Bergens' paper +///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" +/// GJK based Ray Cast, optimized version +/// Objects should not start in overlap, otherwise results are not defined. +class SpuSubsimplexRayCast +{ + SpuVoronoiSimplexSolver* m_simplexSolver; + void* m_shapeB; + SpuConvexPolyhedronVertexData* m_convexDataB; + int m_shapeTypeB; + float m_marginB; + +public: + SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, + SpuVoronoiSimplexSolver* simplexSolver); + + //virtual ~btSubsimplexConvexCast(); + + ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. + ///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector. + bool calcTimeOfImpact(const btTransform& fromRay, + const btTransform& toRay, + const btTransform& fromB, + const btTransform& toB, + SpuCastResult& result); + +}; + +#endif //SUBSIMPLEX_RAY_CAST_H diff --git a/src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp b/src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp index 50dd88d0b..a605c6f48 100644 --- a/src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp +++ b/src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp @@ -1,189 +1,189 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "SpuRaycastTaskProcess.h" - - -SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks) -:m_threadInterface(threadInterface), -m_maxNumOutstandingTasks(maxNumOutstandingTasks) -{ - m_workUnitTaskBuffers = (unsigned char *)0; - m_taskBusy.resize(m_maxNumOutstandingTasks); - m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks); - - for (int i = 0; i < m_maxNumOutstandingTasks; i++) - { - m_taskBusy[i] = false; - } - m_numBusyTasks = 0; - m_currentTask = 0; - m_currentWorkUnitInTask = 0; - - m_threadInterface->startSPU(); - - //printf("sizeof vec_float4: %d\n", sizeof(vec_float4)); - //printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput)); - -} - -SpuRaycastTaskProcess::~SpuRaycastTaskProcess() -{ - - if (m_workUnitTaskBuffers != 0) - { - btAlignedFree(m_workUnitTaskBuffers); - m_workUnitTaskBuffers = 0; - } - - m_threadInterface->stopSPU(); -} - - - -void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers) -{ - m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers; - m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers; - for (int i = 0; i < m_maxNumOutstandingTasks; i++) - { - m_taskBusy[i] = false; - } - m_numBusyTasks = 0; - m_currentTask = 0; - m_currentWorkUnitInTask = 0; - -#ifdef DEBUG_SpuRaycastTaskProcess - m_initialized = true; -#endif -} - - -void SpuRaycastTaskProcess::issueTask2() -{ - m_taskBusy[m_currentTask] = true; - m_numBusyTasks++; - - SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask]; - - taskDesc.taskId = m_currentTask; - m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask); - //printf("send thread requested for task %d\n", m_currentTask); - // if all tasks busy, wait for spu event to clear the task. - if (m_numBusyTasks >= m_maxNumOutstandingTasks) - { - unsigned int taskId; - unsigned int outputSize; - - for (int i=0;iwaitForResponse(&taskId, &outputSize); - - //printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); - - m_taskBusy[taskId] = false; - - m_numBusyTasks--; - } else { - //printf("Sent request, not enough busy tasks\n"); - } -} - -void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit) -{ - m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit; - m_currentWorkUnitInTask++; - if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK) - { - m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; - m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; - m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; - //printf("Task buffer full, issuing\n"); - issueTask2 (); - //printf("Returned from issueTask2()\n"); - m_currentWorkUnitInTask = 0; - - // find new task buffer - for (int i = 0; i < m_maxNumOutstandingTasks; i++) - { - if (!m_taskBusy[i]) - { - m_currentTask = i; - //init the task data - break; - } - } - //printf("next task = %d\n", m_currentTask); - } -} - - -void -SpuRaycastTaskProcess::flush2() -{ -#ifdef DEBUG_SPU_TASK_SCHEDULING - printf("\nSpuRaycastTaskProcess::flush()\n"); -#endif //DEBUG_SPU_TASK_SCHEDULING - - // if there's a partially filled task buffer, submit that task - //printf("Flushing... %d remaining\n", m_currentWorkUnitInTask); - if (m_currentWorkUnitInTask > 0) - { - m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; - m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; - m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; - issueTask2(); - m_currentWorkUnitInTask = 0; - } - - - // all tasks are issued, wait for all tasks to be complete - while(m_numBusyTasks > 0) - { - // Consolidating SPU code - unsigned int taskId; - unsigned int outputSize; - - for (int i=0;iwaitForResponse(&taskId, &outputSize); - } - - //printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); - - //postProcess(taskId, outputSize); - - m_taskBusy[taskId] = false; - - m_numBusyTasks--; - } -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SpuRaycastTaskProcess.h" + + +SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks) +:m_threadInterface(threadInterface), +m_maxNumOutstandingTasks(maxNumOutstandingTasks) +{ + m_workUnitTaskBuffers = (unsigned char *)0; + m_taskBusy.resize(m_maxNumOutstandingTasks); + m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks); + + for (int i = 0; i < m_maxNumOutstandingTasks; i++) + { + m_taskBusy[i] = false; + } + m_numBusyTasks = 0; + m_currentTask = 0; + m_currentWorkUnitInTask = 0; + + m_threadInterface->startSPU(); + + //printf("sizeof vec_float4: %d\n", sizeof(vec_float4)); + //printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput)); + +} + +SpuRaycastTaskProcess::~SpuRaycastTaskProcess() +{ + + if (m_workUnitTaskBuffers != 0) + { + btAlignedFree(m_workUnitTaskBuffers); + m_workUnitTaskBuffers = 0; + } + + m_threadInterface->stopSPU(); +} + + + +void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers) +{ + m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers; + m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers; + for (int i = 0; i < m_maxNumOutstandingTasks; i++) + { + m_taskBusy[i] = false; + } + m_numBusyTasks = 0; + m_currentTask = 0; + m_currentWorkUnitInTask = 0; + +#ifdef DEBUG_SpuRaycastTaskProcess + m_initialized = true; +#endif +} + + +void SpuRaycastTaskProcess::issueTask2() +{ + m_taskBusy[m_currentTask] = true; + m_numBusyTasks++; + + SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask]; + + taskDesc.taskId = m_currentTask; + m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask); + //printf("send thread requested for task %d\n", m_currentTask); + // if all tasks busy, wait for spu event to clear the task. + if (m_numBusyTasks >= m_maxNumOutstandingTasks) + { + unsigned int taskId; + unsigned int outputSize; + + for (int i=0;iwaitForResponse(&taskId, &outputSize); + + //printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); + + m_taskBusy[taskId] = false; + + m_numBusyTasks--; + } else { + //printf("Sent request, not enough busy tasks\n"); + } +} + +void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit) +{ + m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit; + m_currentWorkUnitInTask++; + if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK) + { + m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; + m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; + m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; + //printf("Task buffer full, issuing\n"); + issueTask2 (); + //printf("Returned from issueTask2()\n"); + m_currentWorkUnitInTask = 0; + + // find new task buffer + for (int i = 0; i < m_maxNumOutstandingTasks; i++) + { + if (!m_taskBusy[i]) + { + m_currentTask = i; + //init the task data + break; + } + } + //printf("next task = %d\n", m_currentTask); + } +} + + +void +SpuRaycastTaskProcess::flush2() +{ +#ifdef DEBUG_SPU_TASK_SCHEDULING + printf("\nSpuRaycastTaskProcess::flush()\n"); +#endif //DEBUG_SPU_TASK_SCHEDULING + + // if there's a partially filled task buffer, submit that task + //printf("Flushing... %d remaining\n", m_currentWorkUnitInTask); + if (m_currentWorkUnitInTask > 0) + { + m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; + m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; + m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; + issueTask2(); + m_currentWorkUnitInTask = 0; + } + + + // all tasks are issued, wait for all tasks to be complete + while(m_numBusyTasks > 0) + { + // Consolidating SPU code + unsigned int taskId; + unsigned int outputSize; + + for (int i=0;iwaitForResponse(&taskId, &outputSize); + } + + //printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); + + //postProcess(taskId, outputSize); + + m_taskBusy[taskId] = false; + + m_numBusyTasks--; + } +} diff --git a/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp b/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp index 17b81965e..fe6195557 100644 --- a/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp +++ b/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp @@ -1,214 +1,214 @@ -/* -Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - - -#include "SpuSampleTask.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "../PlatformDefinitions.h" -#include "../SpuFakeDma.h" -#include "LinearMath/btMinMax.h" - -#ifdef __SPU__ -#include -#else -#include -#define spu_printf printf -#endif - -#define MAX_NUM_BODIES 8192 - -struct SampleTask_LocalStoreMemory -{ - ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]); - ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]); - -}; - - - - -//-- MAIN METHOD -void processSampleTask(void* userPtr, void* lsMemory) -{ - // BT_PROFILE("processSampleTask"); - - SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory; - - SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr; - SpuSampleTaskDesc& taskDesc = *taskDescPtr; - - switch (taskDesc.m_sampleCommand) - { - case CMD_SAMPLE_INTEGRATE_BODIES: - { - btTransform predictedTrans; - btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; - - int batchSize = taskDesc.m_sampleValue; - if (batchSize>MAX_NUM_BODIES) - { - spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); - break; - } - int dmaArraySize = batchSize*sizeof(void*); - - uint64_t ppuArrayAddress = reinterpret_cast(eaPtr); - - // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); - - if (dmaArraySize>=16) - { - cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } else - { - stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); - } - - - for ( int i=0;igLocalRigidBody[0]; - void* shortAdd = localMemory->gPointerArray[i]; - uint64_t ppuRigidBodyAddress = reinterpret_cast(shortAdd); - - // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); - - int dmaBodySize = sizeof(btRigidBody); - - cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - float timeStep = 1.f/60.f; - - btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); - if (body) - { - if (body->isActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - void* ptr = (void*)localPtr; - // spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress); - - cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - } - } - - } - break; - } - - - case CMD_SAMPLE_PREDICT_MOTION_BODIES: - { - btTransform predictedTrans; - btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; - - int batchSize = taskDesc.m_sampleValue; - int dmaArraySize = batchSize*sizeof(void*); - - if (batchSize>MAX_NUM_BODIES) - { - spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); - break; - } - - uint64_t ppuArrayAddress = reinterpret_cast(eaPtr); - - // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); - - if (dmaArraySize>=16) - { - cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } else - { - stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); - } - - - for ( int i=0;igLocalRigidBody[0]; - void* shortAdd = localMemory->gPointerArray[i]; - uint64_t ppuRigidBodyAddress = reinterpret_cast(shortAdd); - - // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); - - int dmaBodySize = sizeof(btRigidBody); - - cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - - - float timeStep = 1.f/60.f; - - btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); - if (body) - { - if (!body->isStaticOrKinematicObject()) - { - if (body->isActive()) - { - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); - - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - - void* ptr = (void*)localPtr; - cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); - cellDmaWaitTagStatusAll(DMA_MASK(1)); - } - } - } - - } - break; - } - - - - default: - { - - } - }; -} - - -#if defined(__CELLOS_LV2__) || defined (LIBSPE2) - -ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory); - -void* createSampleLocalStoreMemory() -{ - return &gLocalStoreMemory; -} -#else -void* createSampleLocalStoreMemory() -{ - return new SampleTask_LocalStoreMemory; -}; - -#endif +/* +Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + + +#include "SpuSampleTask.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "../PlatformDefinitions.h" +#include "../SpuFakeDma.h" +#include "LinearMath/btMinMax.h" + +#ifdef __SPU__ +#include +#else +#include +#define spu_printf printf +#endif + +#define MAX_NUM_BODIES 8192 + +struct SampleTask_LocalStoreMemory +{ + ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]); + ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]); + +}; + + + + +//-- MAIN METHOD +void processSampleTask(void* userPtr, void* lsMemory) +{ + // BT_PROFILE("processSampleTask"); + + SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory; + + SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr; + SpuSampleTaskDesc& taskDesc = *taskDescPtr; + + switch (taskDesc.m_sampleCommand) + { + case CMD_SAMPLE_INTEGRATE_BODIES: + { + btTransform predictedTrans; + btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; + + int batchSize = taskDesc.m_sampleValue; + if (batchSize>MAX_NUM_BODIES) + { + spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); + break; + } + int dmaArraySize = batchSize*sizeof(void*); + + uint64_t ppuArrayAddress = reinterpret_cast(eaPtr); + + // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); + + if (dmaArraySize>=16) + { + cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } else + { + stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); + } + + + for ( int i=0;igLocalRigidBody[0]; + void* shortAdd = localMemory->gPointerArray[i]; + uint64_t ppuRigidBodyAddress = reinterpret_cast(shortAdd); + + // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); + + int dmaBodySize = sizeof(btRigidBody); + + cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + float timeStep = 1.f/60.f; + + btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); + if (body) + { + if (body->isActive() && (!body->isStaticOrKinematicObject())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + body->proceedToTransform( predictedTrans); + void* ptr = (void*)localPtr; + // spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress); + + cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + } + } + + } + break; + } + + + case CMD_SAMPLE_PREDICT_MOTION_BODIES: + { + btTransform predictedTrans; + btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; + + int batchSize = taskDesc.m_sampleValue; + int dmaArraySize = batchSize*sizeof(void*); + + if (batchSize>MAX_NUM_BODIES) + { + spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); + break; + } + + uint64_t ppuArrayAddress = reinterpret_cast(eaPtr); + + // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); + + if (dmaArraySize>=16) + { + cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } else + { + stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); + } + + + for ( int i=0;igLocalRigidBody[0]; + void* shortAdd = localMemory->gPointerArray[i]; + uint64_t ppuRigidBodyAddress = reinterpret_cast(shortAdd); + + // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); + + int dmaBodySize = sizeof(btRigidBody); + + cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + + + float timeStep = 1.f/60.f; + + btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); + if (body) + { + if (!body->isStaticOrKinematicObject()) + { + if (body->isActive()) + { + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); + + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + + void* ptr = (void*)localPtr; + cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); + cellDmaWaitTagStatusAll(DMA_MASK(1)); + } + } + } + + } + break; + } + + + + default: + { + + } + }; +} + + +#if defined(__CELLOS_LV2__) || defined (LIBSPE2) + +ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory); + +void* createSampleLocalStoreMemory() +{ + return &gLocalStoreMemory; +} +#else +void* createSampleLocalStoreMemory() +{ + return new SampleTask_LocalStoreMemory; +}; + +#endif diff --git a/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h b/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h index 900be99c4..c8ebdfd62 100644 --- a/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h +++ b/src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h @@ -1,54 +1,54 @@ -/* -Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. - -*/ - -#ifndef SPU_SAMPLE_TASK_H -#define SPU_SAMPLE_TASK_H - -#include "../PlatformDefinitions.h" -#include "LinearMath/btScalar.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" - -#include "LinearMath/btAlignedAllocator.h" - - -enum -{ - CMD_SAMPLE_INTEGRATE_BODIES = 1, - CMD_SAMPLE_PREDICT_MOTION_BODIES -}; - - - -ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc -{ - BT_DECLARE_ALIGNED_ALLOCATOR(); - - uint32_t m_sampleCommand; - uint32_t m_taskId; - - uint64_t m_mainMemoryPtr; - int m_sampleValue; - - -}; - - -void processSampleTask(void* userPtr, void* lsMemory); -void* createSampleLocalStoreMemory(); - - -#endif //SPU_SAMPLE_TASK_H - +/* +Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + +*/ + +#ifndef SPU_SAMPLE_TASK_H +#define SPU_SAMPLE_TASK_H + +#include "../PlatformDefinitions.h" +#include "LinearMath/btScalar.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + +#include "LinearMath/btAlignedAllocator.h" + + +enum +{ + CMD_SAMPLE_INTEGRATE_BODIES = 1, + CMD_SAMPLE_PREDICT_MOTION_BODIES +}; + + + +ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc +{ + BT_DECLARE_ALIGNED_ALLOCATOR(); + + uint32_t m_sampleCommand; + uint32_t m_taskId; + + uint64_t m_mainMemoryPtr; + int m_sampleValue; + + +}; + + +void processSampleTask(void* userPtr, void* lsMemory); +void* createSampleLocalStoreMemory(); + + +#endif //SPU_SAMPLE_TASK_H + diff --git a/src/BulletMultiThreaded/SpuSampleTask/readme.txt b/src/BulletMultiThreaded/SpuSampleTask/readme.txt index f3e907347..5b4a90705 100644 --- a/src/BulletMultiThreaded/SpuSampleTask/readme.txt +++ b/src/BulletMultiThreaded/SpuSampleTask/readme.txt @@ -1 +1 @@ -Empty placeholder for future Libspe2 SPU task +Empty placeholder for future Libspe2 SPU task diff --git a/src/BulletMultiThreaded/Win32ThreadSupport.cpp b/src/BulletMultiThreaded/Win32ThreadSupport.cpp index 8aa4cdce6..95363c158 100644 --- a/src/BulletMultiThreaded/Win32ThreadSupport.cpp +++ b/src/BulletMultiThreaded/Win32ThreadSupport.cpp @@ -1,259 +1,259 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "Win32ThreadSupport.h" - -#ifdef USE_WIN32_THREADING - -#include - -#include "SpuCollisionTaskProcess.h" - -#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" - - - -///The number of threads should be equal to the number of available cores -///@todo: each worker should be linked to a single core, using SetThreadIdealProcessor. - -///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication -///Setup and initialize SPU/CELL/Libspe2 -Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo) -{ - startThreads(threadConstructionInfo); -} - -///cleanup/shutdown Libspe2 -Win32ThreadSupport::~Win32ThreadSupport() -{ - stopSPU(); -} - - - - -#include - -DWORD WINAPI Thread_no_1( LPVOID lpParam ) -{ - - Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam; - - - while (1) - { - WaitForSingleObject(status->m_eventStartHandle,INFINITE); - - void* userPtr = status->m_userPtr; - - if (userPtr) - { - btAssert(status->m_status); - status->m_userThreadFunc(userPtr,status->m_lsMemory); - status->m_status = 2; - SetEvent(status->m_eventCompletetHandle); - } else - { - //exit Thread - status->m_status = 3; - SetEvent(status->m_eventCompletetHandle); - printf("Thread with taskId %i with handle %p exiting\n",status->m_taskId, status->m_threadHandle); - break; - } - - } - - printf("Thread TERMINATED\n"); - return 0; - -} - -///send messages to SPUs -void Win32ThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) -{ - /// gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (ppu_address_t) &taskDesc); - - ///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished - - - - switch (uiCommand) - { - case CMD_GATHER_AND_PROCESS_PAIRLIST: - { - - -//#define SINGLE_THREADED 1 -#ifdef SINGLE_THREADED - - btSpuStatus& spuStatus = m_activeSpuStatus[0]; - spuStatus.m_userPtr=(void*)uiArgument0; - spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); - HANDLE handle =0; -#else - - - btSpuStatus& spuStatus = m_activeSpuStatus[taskId]; - btAssert(taskId>=0); - btAssert(taskId 1); - spuStatus.m_status = 0; - - ///need to find an active spu - btAssert(last>=0); - -#else - last=0; - btSpuStatus& spuStatus = m_activeSpuStatus[last]; -#endif //SINGLE_THREADED - - - - *puiArgument0 = spuStatus.m_taskId; - *puiArgument1 = spuStatus.m_status; - - -} - - - -void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo) -{ - - m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads); - m_completeHandles.resize(threadConstructionInfo.m_numThreads); - - for (int i=0;i0) - { - WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); - } - - - spuStatus.m_userPtr = 0; - SetEvent(spuStatus.m_eventStartHandle); - WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); - - CloseHandle(spuStatus.m_eventCompletetHandle); - CloseHandle(spuStatus.m_eventStartHandle); - CloseHandle(spuStatus.m_threadHandle); - } - - m_activeSpuStatus.clear(); - m_completeHandles.clear(); - -} - -#endif //USE_WIN32_THREADING +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "Win32ThreadSupport.h" + +#ifdef USE_WIN32_THREADING + +#include + +#include "SpuCollisionTaskProcess.h" + +#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" + + + +///The number of threads should be equal to the number of available cores +///@todo: each worker should be linked to a single core, using SetThreadIdealProcessor. + +///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication +///Setup and initialize SPU/CELL/Libspe2 +Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo) +{ + startThreads(threadConstructionInfo); +} + +///cleanup/shutdown Libspe2 +Win32ThreadSupport::~Win32ThreadSupport() +{ + stopSPU(); +} + + + + +#include + +DWORD WINAPI Thread_no_1( LPVOID lpParam ) +{ + + Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam; + + + while (1) + { + WaitForSingleObject(status->m_eventStartHandle,INFINITE); + + void* userPtr = status->m_userPtr; + + if (userPtr) + { + btAssert(status->m_status); + status->m_userThreadFunc(userPtr,status->m_lsMemory); + status->m_status = 2; + SetEvent(status->m_eventCompletetHandle); + } else + { + //exit Thread + status->m_status = 3; + SetEvent(status->m_eventCompletetHandle); + printf("Thread with taskId %i with handle %p exiting\n",status->m_taskId, status->m_threadHandle); + break; + } + + } + + printf("Thread TERMINATED\n"); + return 0; + +} + +///send messages to SPUs +void Win32ThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) +{ + /// gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (ppu_address_t) &taskDesc); + + ///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished + + + + switch (uiCommand) + { + case CMD_GATHER_AND_PROCESS_PAIRLIST: + { + + +//#define SINGLE_THREADED 1 +#ifdef SINGLE_THREADED + + btSpuStatus& spuStatus = m_activeSpuStatus[0]; + spuStatus.m_userPtr=(void*)uiArgument0; + spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); + HANDLE handle =0; +#else + + + btSpuStatus& spuStatus = m_activeSpuStatus[taskId]; + btAssert(taskId>=0); + btAssert(taskId 1); + spuStatus.m_status = 0; + + ///need to find an active spu + btAssert(last>=0); + +#else + last=0; + btSpuStatus& spuStatus = m_activeSpuStatus[last]; +#endif //SINGLE_THREADED + + + + *puiArgument0 = spuStatus.m_taskId; + *puiArgument1 = spuStatus.m_status; + + +} + + + +void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo) +{ + + m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads); + m_completeHandles.resize(threadConstructionInfo.m_numThreads); + + for (int i=0;i0) + { + WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); + } + + + spuStatus.m_userPtr = 0; + SetEvent(spuStatus.m_eventStartHandle); + WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); + + CloseHandle(spuStatus.m_eventCompletetHandle); + CloseHandle(spuStatus.m_eventStartHandle); + CloseHandle(spuStatus.m_threadHandle); + } + + m_activeSpuStatus.clear(); + m_completeHandles.clear(); + +} + +#endif //USE_WIN32_THREADING diff --git a/src/BulletMultiThreaded/Win32ThreadSupport.h b/src/BulletMultiThreaded/Win32ThreadSupport.h index 183cd2e3a..182bf82f1 100644 --- a/src/BulletMultiThreaded/Win32ThreadSupport.h +++ b/src/BulletMultiThreaded/Win32ThreadSupport.h @@ -1,125 +1,125 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "LinearMath/btScalar.h" -#include "PlatformDefinitions.h" - -#ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h - -#ifndef WIN32_THREAD_SUPPORT_H -#define WIN32_THREAD_SUPPORT_H - -#include "LinearMath/btAlignedObjectArray.h" - -#include "btThreadSupportInterface.h" - - -typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory); -typedef void* (*Win32lsMemorySetupFunc)(); - - - - - - -///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication -class Win32ThreadSupport : public btThreadSupportInterface -{ -public: - ///placeholder, until libspe2 support is there - struct btSpuStatus - { - uint32_t m_taskId; - uint32_t m_commandId; - uint32_t m_status; - - Win32ThreadFunc m_userThreadFunc; - void* m_userPtr; //for taskDesc etc - void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc - - void* m_threadHandle; //this one is calling 'Win32ThreadFunc' - - void* m_eventStartHandle; - char m_eventStartHandleName[32]; - - void* m_eventCompletetHandle; - char m_eventCompletetHandleName[32]; - - - }; -private: - - btAlignedObjectArray m_activeSpuStatus; - btAlignedObjectArray m_completeHandles; - -public: - ///Setup and initialize SPU/CELL/Libspe2 - - struct Win32ThreadConstructionInfo - { - Win32ThreadConstructionInfo(char* uniqueName, - Win32ThreadFunc userThreadFunc, - Win32lsMemorySetupFunc lsMemoryFunc, - int numThreads=1, - int threadStackSize=65535 - ) - :m_uniqueName(uniqueName), - m_userThreadFunc(userThreadFunc), - m_lsMemoryFunc(lsMemoryFunc), - m_numThreads(numThreads), - m_threadStackSize(threadStackSize) - { - - } - - char* m_uniqueName; - Win32ThreadFunc m_userThreadFunc; - Win32lsMemorySetupFunc m_lsMemoryFunc; - int m_numThreads; - int m_threadStackSize; - - }; - - - - Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo); - -///cleanup/shutdown Libspe2 - virtual ~Win32ThreadSupport(); - - void startThreads(const Win32ThreadConstructionInfo& threadInfo); - - -///send messages to SPUs - virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); - -///check for messages from SPUs - virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); - -///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded) - virtual void startSPU(); - -///tell the task scheduler we are done with the SPU tasks - virtual void stopSPU(); - - virtual void setNumTasks(int numTasks) - { - } - -}; - -#endif //WIN32_THREAD_SUPPORT_H - -#endif //USE_WIN32_THREADING +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btScalar.h" +#include "PlatformDefinitions.h" + +#ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h + +#ifndef WIN32_THREAD_SUPPORT_H +#define WIN32_THREAD_SUPPORT_H + +#include "LinearMath/btAlignedObjectArray.h" + +#include "btThreadSupportInterface.h" + + +typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory); +typedef void* (*Win32lsMemorySetupFunc)(); + + + + + + +///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication +class Win32ThreadSupport : public btThreadSupportInterface +{ +public: + ///placeholder, until libspe2 support is there + struct btSpuStatus + { + uint32_t m_taskId; + uint32_t m_commandId; + uint32_t m_status; + + Win32ThreadFunc m_userThreadFunc; + void* m_userPtr; //for taskDesc etc + void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc + + void* m_threadHandle; //this one is calling 'Win32ThreadFunc' + + void* m_eventStartHandle; + char m_eventStartHandleName[32]; + + void* m_eventCompletetHandle; + char m_eventCompletetHandleName[32]; + + + }; +private: + + btAlignedObjectArray m_activeSpuStatus; + btAlignedObjectArray m_completeHandles; + +public: + ///Setup and initialize SPU/CELL/Libspe2 + + struct Win32ThreadConstructionInfo + { + Win32ThreadConstructionInfo(char* uniqueName, + Win32ThreadFunc userThreadFunc, + Win32lsMemorySetupFunc lsMemoryFunc, + int numThreads=1, + int threadStackSize=65535 + ) + :m_uniqueName(uniqueName), + m_userThreadFunc(userThreadFunc), + m_lsMemoryFunc(lsMemoryFunc), + m_numThreads(numThreads), + m_threadStackSize(threadStackSize) + { + + } + + char* m_uniqueName; + Win32ThreadFunc m_userThreadFunc; + Win32lsMemorySetupFunc m_lsMemoryFunc; + int m_numThreads; + int m_threadStackSize; + + }; + + + + Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo); + +///cleanup/shutdown Libspe2 + virtual ~Win32ThreadSupport(); + + void startThreads(const Win32ThreadConstructionInfo& threadInfo); + + +///send messages to SPUs + virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); + +///check for messages from SPUs + virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); + +///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded) + virtual void startSPU(); + +///tell the task scheduler we are done with the SPU tasks + virtual void stopSPU(); + + virtual void setNumTasks(int numTasks) + { + } + +}; + +#endif //WIN32_THREAD_SUPPORT_H + +#endif //USE_WIN32_THREADING diff --git a/src/BulletMultiThreaded/vectormath2bullet.h b/src/BulletMultiThreaded/vectormath2bullet.h index 9bc0d20f8..737095e9c 100644 --- a/src/BulletMultiThreaded/vectormath2bullet.h +++ b/src/BulletMultiThreaded/vectormath2bullet.h @@ -1,73 +1,73 @@ -/* - Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. - All rights reserved. - - Redistribution and use in source and binary forms, - with or without modification, are permitted provided that the - following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Sony Computer Entertainment Inc nor the names - of its contributors may be used to endorse or promote products derived - from this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef AOS_VECTORMATH_BULLET_CONVERT_H -#define AOS_VECTORMATH_BULLET_CONVERT_H - -#include -#include "LinearMath/btVector3.h" -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btMatrix3x3.h" - -inline Vectormath::Aos::Vector3 getVmVector3(const btVector3& bulletVec) -{ - return Vectormath::Aos::Vector3(bulletVec.getX(),bulletVec.getY(),bulletVec.getZ()); -} - -inline btVector3 getBtVector3(const Vectormath::Aos::Vector3& vmVec) -{ - return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); -} -inline btVector3 getBtVector3(const Vectormath::Aos::Point3& vmVec) -{ - return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); -} - -inline Vectormath::Aos::Quat getVmQuat(const btQuaternion& bulletQuat) -{ - Vectormath::Aos::Quat vmQuat(bulletQuat.getX(),bulletQuat.getY(),bulletQuat.getZ(),bulletQuat.getW()); - return vmQuat; -} - -inline btQuaternion getBtQuat(const Vectormath::Aos::Quat& vmQuat) -{ - return btQuaternion (vmQuat.getX(),vmQuat.getY(),vmQuat.getZ(),vmQuat.getW()); -} - -inline Vectormath::Aos::Matrix3 getVmMatrix3(const btMatrix3x3& btMat) -{ - Vectormath::Aos::Matrix3 mat( - getVmVector3(btMat.getColumn(0)), - getVmVector3(btMat.getColumn(1)), - getVmVector3(btMat.getColumn(2))); - return mat; -} - - -#endif //AOS_VECTORMATH_BULLET_CONVERT_H +/* + Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. + All rights reserved. + + Redistribution and use in source and binary forms, + with or without modification, are permitted provided that the + following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Sony Computer Entertainment Inc nor the names + of its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef AOS_VECTORMATH_BULLET_CONVERT_H +#define AOS_VECTORMATH_BULLET_CONVERT_H + +#include +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btMatrix3x3.h" + +inline Vectormath::Aos::Vector3 getVmVector3(const btVector3& bulletVec) +{ + return Vectormath::Aos::Vector3(bulletVec.getX(),bulletVec.getY(),bulletVec.getZ()); +} + +inline btVector3 getBtVector3(const Vectormath::Aos::Vector3& vmVec) +{ + return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); +} +inline btVector3 getBtVector3(const Vectormath::Aos::Point3& vmVec) +{ + return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); +} + +inline Vectormath::Aos::Quat getVmQuat(const btQuaternion& bulletQuat) +{ + Vectormath::Aos::Quat vmQuat(bulletQuat.getX(),bulletQuat.getY(),bulletQuat.getZ(),bulletQuat.getW()); + return vmQuat; +} + +inline btQuaternion getBtQuat(const Vectormath::Aos::Quat& vmQuat) +{ + return btQuaternion (vmQuat.getX(),vmQuat.getY(),vmQuat.getZ(),vmQuat.getW()); +} + +inline Vectormath::Aos::Matrix3 getVmMatrix3(const btMatrix3x3& btMat) +{ + Vectormath::Aos::Matrix3 mat( + getVmVector3(btMat.getColumn(0)), + getVmVector3(btMat.getColumn(1)), + getVmVector3(btMat.getColumn(2))); + return mat; +} + + +#endif //AOS_VECTORMATH_BULLET_CONVERT_H diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 435b0cdb7..8217a87a8 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1,2649 +1,2649 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#include "btSoftBodyInternals.h" - -// -btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_worldInfo(worldInfo) -{ - /* Init */ - m_internalType = CO_SOFT_BODY; - m_cfg.aeromodel = eAeroModel::V_Point; - m_cfg.kVCF = 1; - m_cfg.kDG = 0; - m_cfg.kLF = 0; - m_cfg.kDP = 0; - m_cfg.kPR = 0; - m_cfg.kVC = 0; - m_cfg.kDF = (btScalar)0.2; - m_cfg.kMT = 0; - m_cfg.kCHR = (btScalar)1.0; - m_cfg.kKHR = (btScalar)0.1; - m_cfg.kSHR = (btScalar)1.0; - m_cfg.kAHR = (btScalar)0.7; - m_cfg.kSRHR_CL = (btScalar)0.1; - m_cfg.kSKHR_CL = (btScalar)1; - m_cfg.kSSHR_CL = (btScalar)0.5; - m_cfg.kSR_SPLT_CL = (btScalar)0.5; - m_cfg.kSK_SPLT_CL = (btScalar)0.5; - m_cfg.kSS_SPLT_CL = (btScalar)0.5; - m_cfg.maxvolume = (btScalar)1; - m_cfg.timescale = 1; - m_cfg.viterations = 0; - m_cfg.piterations = 1; - m_cfg.diterations = 0; - m_cfg.citerations = 4; - m_cfg.collisions = fCollision::Default; - m_pose.m_bvolume = false; - m_pose.m_bframe = false; - m_pose.m_volume = 0; - m_pose.m_com = btVector3(0,0,0); - m_pose.m_rot.setIdentity(); - m_pose.m_scl.setIdentity(); - m_tag = 0; - m_timeacc = 0; - m_bUpdateRtCst = true; - m_bounds[0] = btVector3(0,0,0); - m_bounds[1] = btVector3(0,0,0); - m_worldTransform.setIdentity(); - setSolver(eSolverPresets::Positions); - /* Default material */ - Material* pm=appendMaterial(); - pm->m_kLST = 1; - pm->m_kAST = 1; - pm->m_kVST = 1; - pm->m_flags = fMaterial::Default; - /* Collision shape */ - ///for now, create a collision shape internally - m_collisionShape = new btSoftBodyCollisionShape(this); - m_collisionShape->setMargin(0.25); - /* Nodes */ - const btScalar margin=getCollisionShape()->getMargin(); - m_nodes.resize(node_count); - for(int i=0,ni=node_count;i0?1/n.m_im:0; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); - n.m_material= pm; - } - updateBounds(); - - -} - -// -btSoftBody::~btSoftBody() -{ - //for now, delete the internal shape - delete m_collisionShape; - int i; - - releaseClusters(); - for(i=0;i0) - *pm=*m_materials[0]; - else - ZeroInitialize(*pm); - m_materials.push_back(pm); - return(pm); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) -{ - Note n; - ZeroInitialize(n); - n.m_rank = 0; - n.m_text = text; - n.m_offset = o; - n.m_coords[0] = c.x(); - n.m_coords[1] = c.y(); - n.m_coords[2] = c.z(); - n.m_coords[3] = c.w(); - n.m_nodes[0] = n0;n.m_rank+=n0?1:0; - n.m_nodes[1] = n1;n.m_rank+=n1?1:0; - n.m_nodes[2] = n2;n.m_rank+=n2?1:0; - n.m_nodes[3] = n3;n.m_rank+=n3?1:0; - m_notes.push_back(n); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) -{ - appendNote(text,o,btVector4(1,0,0,0),feature); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) -{ - static const btScalar w=1/(btScalar)2; - appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); -} - -// -void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) -{ - static const btScalar w=1/(btScalar)3; - appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); -} - -// -void btSoftBody::appendNode( const btVector3& x,btScalar m) -{ - if(m_nodes.capacity()==m_nodes.size()) - { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); - } - const btScalar margin=getCollisionShape()->getMargin(); - m_nodes.push_back(Node()); - Node& n=m_nodes[m_nodes.size()-1]; - ZeroInitialize(n); - n.m_x = x; - n.m_q = n.m_x; - n.m_im = m>0?1/m:0; - n.m_material = m_materials[0]; - n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); -} - -// -void btSoftBody::appendLink(int model,Material* mat) -{ - Link l; - if(model>=0) - l=m_links[model]; - else - { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } - m_links.push_back(l); -} - -// -void btSoftBody::appendLink( int node0, - int node1, - Material* mat, - bool bcheckexist) -{ - appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); -} - -// -void btSoftBody::appendLink( Node* node0, - Node* node1, - Material* mat, - bool bcheckexist) -{ - if((!bcheckexist)||(!checkLink(node0,node1))) - { - appendLink(-1,mat); - Link& l=m_links[m_links.size()-1]; - l.m_n[0] = node0; - l.m_n[1] = node1; - l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); - m_bUpdateRtCst=true; - } -} - -// -void btSoftBody::appendFace(int model,Material* mat) -{ - Face f; - if(model>=0) - { f=m_faces[model]; } - else - { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } - m_faces.push_back(f); -} - -// -void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) -{ - if (node0==node1) - return; - if (node1==node2) - return; - if (node2==node0) - return; - - appendFace(-1,mat); - Face& f=m_faces[m_faces.size()-1]; - btAssert(node0!=node1); - btAssert(node1!=node2); - btAssert(node2!=node0); - f.m_n[0] = &m_nodes[node0]; - f.m_n[1] = &m_nodes[node1]; - f.m_n[2] = &m_nodes[node2]; - f.m_ra = AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - m_bUpdateRtCst=true; -} - -// -void btSoftBody::appendAnchor(int node,btRigidBody* body) -{ - Anchor a; - a.m_node = &m_nodes[node]; - a.m_body = body; - a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x; - a.m_node->m_battach = 1; - m_anchors.push_back(a); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) -{ - LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) -{ - appendLinearJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) -{ - appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) -{ - AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); - pj->m_bodies[0] = body0; - pj->m_bodies[1] = body1; - pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; - pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; - pj->m_cfm = specs.cfm; - pj->m_erp = specs.erp; - pj->m_split = specs.split; - pj->m_icontrol = specs.icontrol; - m_joints.push_back(pj); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) -{ - appendAngularJoint(specs,m_clusters[0],body); -} - -// -void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) -{ - appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); -} - -// -void btSoftBody::addForce(const btVector3& force) -{ - for(int i=0,ni=m_nodes.size();i0) - { - n.m_f += force; - } -} - -// -void btSoftBody::addVelocity(const btVector3& velocity) -{ - for(int i=0,ni=m_nodes.size();i0) - { - n.m_v = velocity; - } - } -} - - -// -void btSoftBody::addVelocity(const btVector3& velocity,int node) -{ - Node& n=m_nodes[node]; - if(n.m_im>0) - { - n.m_v += velocity; - } -} - -// -void btSoftBody::setMass(int node,btScalar mass) -{ - m_nodes[node].m_im=mass>0?1/mass:0; - m_bUpdateRtCst=true; -} - -// -btScalar btSoftBody::getMass(int node) const -{ - return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); -} - -// -btScalar btSoftBody::getTotalMass() const -{ - btScalar mass=0; - for(int i=0;im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); - for(int j=0;j<3;++j) - { - f.m_n[j]->m_im+=twicearea; - } - } - for( i=0;igetMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();igetMargin(); - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; - - for(int i=0,ni=m_nodes.size();i0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; - } - /* Pos */ - const btVector3 com=evaluateCom(); - m_pose.m_pos.resize(m_nodes.size()); - for( i=0,ni=m_nodes.size();i0) - { - int i,ni; - - const btVector3 org=m_nodes[0].m_x; - for(i=0,ni=m_faces.size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); - } - vol/=(btScalar)6; - } - return(vol); -} - -// -int btSoftBody::clusterCount() const -{ - return(m_clusters.size()); -} - -// -btVector3 btSoftBody::clusterCom(const Cluster* cluster) -{ - btVector3 com(0,0,0); - for(int i=0,ni=cluster->m_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; - } - return(com*cluster->m_imass); -} - -// -btVector3 btSoftBody::clusterCom(int cluster) const -{ - return(clusterCom(m_clusters[cluster])); -} - -// -btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) -{ - return(cluster->m_lv+cross(cluster->m_av,rpos)); -} - -// -void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) -{ - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); - cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; - cluster->m_nvimpulses++; -} - -// -void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) -{ - const btVector3 li=cluster->m_imass*impulse; - const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); - cluster->m_dimpulses[0]+=li; - cluster->m_dimpulses[1]+=ai; - cluster->m_ndimpulses++; -} - -// -void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) -{ - if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); - if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); -} - -// -void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) -{ - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; - cluster->m_nvimpulses++; -} - -// -void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) -{ - const btVector3 ai=cluster->m_invwi*impulse; - cluster->m_dimpulses[1]+=ai; - cluster->m_ndimpulses++; -} - -// -void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) -{ - if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); - if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); -} - -// -void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) -{ - cluster->m_dimpulses[0]+=impulse*cluster->m_imass; - cluster->m_ndimpulses++; -} - -// -int btSoftBody::generateBendingConstraints(int distance,Material* mat) -{ - int i,j; - - if(distance>1) - { - /* Build graph */ - const int n=m_nodes.size(); - const unsigned inf=(~(unsigned)0)>>1; - unsigned* adj=new unsigned[n*n]; -#define IDX(_x_,_y_) ((_y_)*n+(_x_)) - for(j=0;jsum) - { - adj[IDX(i,j)]=adj[IDX(j,i)]=sum; - } - } - } - } - /* Build links */ - int nlinks=0; - for(j=0;jm_leaf) m_cdbvt.remove(c->m_leaf); - c->~Cluster(); - btAlignedFree(c); - m_clusters.remove(c); -} - -// -void btSoftBody::releaseClusters() -{ - while(m_clusters.size()>0) releaseCluster(0); -} - -// -int btSoftBody::generateClusters(int k,int maxiterations) -{ - int i; - releaseClusters(); - m_clusters.resize(btMin(k,m_nodes.size())); - for(i=0;im_collide= true; - } - k=m_clusters.size(); - if(k>0) - { - /* Initialize */ - btAlignedObjectArray centers; - btVector3 cog(0,0,0); - int i; - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin(1,iterations/slope); - changed=false; - iterations++; - int i; - - for(i=0;im_nodes.size();++j) - { - c+=m_clusters[i]->m_nodes[j]->m_x; - } - if(m_clusters[i]->m_nodes.size()) - { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; - m_clusters[i]->m_nodes.resize(0); - } - } - for(i=0;im_nodes.push_back(&m_nodes[i]); - } - } while(changed&&(iterations cids; - cids.resize(m_nodes.size(),-1); - for(i=0;im_nodes.size();++j) - { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; - } - } - for(i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) - { - m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); - } - } - } - } - } - /* Master */ - if(m_clusters.size()>1) - { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; - pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;im_nodes.push_back(&m_nodes[i]); - m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); - } - /* Terminate */ - for(i=0;im_nodes.size()==0) - { - releaseCluster(i--); - } - } - - initializeClusters(); - updateClusters(); - return(m_clusters.size()); - } - return(0); -} - -// -void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) -{ - const Node* nbase = &m_nodes[0]; - int ncount = m_nodes.size(); - btSymMatrix edges(ncount,-2); - int newnodes=0; - int i,j,k,ni; - - /* Filter out */ - for(i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) - { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; - } - } - } - /* Fill edges */ - for(i=0;i0) - { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) - { - if(b.m_im>0) - { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; - } - else - { a.m_im/=0.5;m=1/a.m_im; } - } - else - { - if(b.m_im>0) - { b.m_im/=0.5;m=1/b.m_im; } - else - m=0; - } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; - ++newnodes; - } - } - } - } - nbase=&m_nodes[0]; - /* Refine links */ - for(i=0,ni=m_links.size();i0) - { - appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; - } - } - } - /* Refine faces */ - for(i=0;i0) - { - appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; - } - } - } - } - /* Cut */ - if(cut) - { - btAlignedObjectArray cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; - } - } - nbase=&m_nodes[0]; - /* Links */ - for(i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; - btAlignedObjectArray todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i=0;--i) - { - if(!ranks[i]) todelete.push_back(i); - } - if(todelete.size()) - { - btAlignedObjectArray& map=ranks; - for(int i=0;im_v=v; - pn[1]->m_v=v; - for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; - } - } - for(i=0,ni=m_faces.size();im_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - } - } - } - if(!done) - { - m_ndbvt.remove(pn[0]->m_leaf); - m_ndbvt.remove(pn[1]->m_leaf); - m_nodes.pop_back(); - m_nodes.pop_back(); - } - return(done); -} - -// -bool btSoftBody::rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results) -{ - if(m_faces.size()&&m_fdbvt.empty()) - initializeFaceTree(); - - results.body = this; - results.fraction = 1.f; - results.feature = eFeature::None; - results.index = -1; - - return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); -} - -// -void btSoftBody::setSolver(eSolverPresets::_ preset) -{ - m_cfg.m_vsequence.clear(); - m_cfg.m_psequence.clear(); - m_cfg.m_dsequence.clear(); - switch(preset) - { - case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; - case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); - - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; - } -} - -// -void btSoftBody::predictMotion(btScalar dt) -{ - int i,ni; - - /* Update */ - if(m_bUpdateRtCst) - { - m_bUpdateRtCst=false; - updateConstants(); - m_fdbvt.clear(); - if(m_cfg.collisions&fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt*m_cfg.timescale; - m_sst.isdt = 1/m_sst.sdt; - m_sst.velmrg = m_sst.sdt*3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; - /* Forces */ - addVelocity(m_worldInfo->m_gravity*m_sst.sdt); - applyForces(); - /* Integrate */ - for(i=0,ni=m_nodes.size();im_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; - vol = VolumeOf(f,m_sst.radmrg); - m_fdbvt.update( f.m_leaf, - vol, - v*m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if(m_pose.m_bframe&&(m_cfg.kMT>0)) - { - const btMatrix3x3 posetrs=m_pose.m_rot; - for(int i=0,ni=m_nodes.size();i0) - { - const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; - n.m_x=Lerp(n.m_x,x,m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); -} - -// -void btSoftBody::solveConstraints() -{ - /* Apply clusters */ - applyClusters(false); - /* Prepare links */ - - int i,ni; - - for(i=0,ni=m_links.size();im_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); - } - /* Prepare anchors */ - for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; - a.m_body->activate(); - } - /* Solve velocities */ - if(m_cfg.viterations>0) - { - /* Solve */ - for(int isolve=0;isolve0) - { - for(int isolve=0;isolve0) - { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i& bodies) -{ - const int nb=bodies.size(); - int iterations=0; - int i; - - for(i=0;im_cfg.citerations); - } - for(i=0;iprepareClusters(iterations); - } - for(i=0;isolveClusters(sor); - } - } - for(i=0;icleanupClusters(); - } -} - -// -void btSoftBody::integrateMotion() -{ - /* Update */ - updateNormals(); -} - -// -btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) -{ - m_rayFrom = rayFrom; - m_rayNormalizedDirection = (rayTo-rayFrom); - m_rayTo = rayTo; - m_mint = mxt; - m_face = 0; - m_tests = 0; -} - -// -void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) -{ - btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; - const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, - f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - m_mint); - if((t>0)&&(tteps)&&(tceps) && - (dot(n,cross(b-hit,c-hit))>ceps) && - (dot(n,cross(c-hit,a-hit))>ceps)) - { - return(t); - } - } - } - return(-1); -} - -// -void btSoftBody::pointersToIndices() -{ -#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) - btSoftBody::Node* base=&m_nodes[0]; - int i,ni; - - for(i=0,ni=m_nodes.size();idata=*(void**)&i; - } - } - for(i=0,ni=m_links.size();idata=*(void**)&i; - } - } - for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; - } - } - for(i=0,ni=m_links.size();idata=&m_faces[i]; - } - } - for(i=0,ni=m_anchors.size();im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); - if(t>0) - { - ++cnt; - if(!bcountonly) - { - feature=btSoftBody::eFeature::Face; - index=i; - mint=t; - } - } - } - } - else - {/* Use dbvt */ - RayFromToCaster collider(rayFrom,rayTo,mint); - - btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); - if(collider.m_face) - { - mint=collider.m_mint; - feature=btSoftBody::eFeature::Face; - index=(int)(collider.m_face-&m_faces[0]); - cnt=1; - } - } - return(cnt); -} - -// -void btSoftBody::initializeFaceTree() -{ - m_fdbvt.clear(); - for(int i=0;igetCollisionShape(); - btRigidBody* tmpRigid = btRigidBody::upcast(colObj); - const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); - btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); - if(dst<0) - { - cti.m_colObj = colObj; - cti.m_normal = wtr.getBasis()*nrm; - cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); - return(true); - } - return(false); -} - -// -void btSoftBody::updateNormals() -{ - const btVector3 zv(0,0,0); - int i,ni; - - for(i=0,ni=m_nodes.size();im_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); - f.m_normal=n.normalized(); - f.m_n[0]->m_n+=n; - f.m_n[1]->m_n+=n; - f.m_n[2]->m_n+=n; - } - for(i=0,ni=m_nodes.size();iSIMD_EPSILON) - m_nodes[i].m_n /= len; - } -} - -// -void btSoftBody::updateBounds() -{ - if(m_ndbvt.m_root) - { - const btVector3& mins=m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); - const btScalar csm=getCollisionShape()->getMargin(); - const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... - m_bounds[0]=mins-mrg; - m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } - } - else - { - m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } -} - - -// -void btSoftBody::updatePose() -{ - if(m_pose.m_bframe) - { - btSoftBody::Pose& pose=m_pose; - const btVector3 com=evaluateCom(); - /* Com */ - pose.m_com = com; - /* Rotation */ - btMatrix3x3 Apq; - const btScalar eps=SIMD_EPSILON; - Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); - Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); - for(int i=0,ni=m_nodes.size();i1) - { - const btScalar idet=Clamp( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); - pose.m_scl=Mul(pose.m_scl,idet); - } - - } -} - -// -void btSoftBody::updateConstants() -{ - int i,ni; - - /* Links */ - for(i=0,ni=m_links.size();im_x-l.m_n[1]->m_x).length(); - l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; - l.m_c1 = l.m_rl*l.m_rl; - } - /* Faces */ - for(i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); - } - /* Area's */ - btAlignedObjectArray counts; - counts.resize(m_nodes.size(),0); - for(i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); - } - } - for(i=0,ni=m_nodes.size();i0) - m_nodes[i].m_area/=(btScalar)counts[i]; - else - m_nodes[i].m_area=0; - } -} - -// -void btSoftBody::initializeClusters() -{ - int i; - - for( i=0;im_im>0?1/c.m_nodes[j]->m_im:0; - c.m_imass += c.m_masses[j]; - } - c.m_imass = 1/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); - { - int i,ni; - - for(i=0,ni=c.m_nodes.size();im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; - } - } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - ii=ii.inverse(); - /* Frame */ - c.m_framexform.setIdentity(); - c.m_framexform.setOrigin(c.m_com); - c.m_framerefs.resize(c.m_nodes.size()); - { - int i; - for(i=0;im_x-c.m_com; - } - } - } -} - -// -void btSoftBody::updateClusters() -{ - BT_PROFILE("UpdateClusters"); - int i; - - for(i=0;im_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; - } - PolarDecompose(m,r,s); - c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ -#if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); -#else -#if 0/* Sphere */ - const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); - const btVector3 inertia(rk,rk,rk); - const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, - btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, - btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); - - c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); -#else/* Actual */ - c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); - for(int i=0;im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=1/c.m_nodes[i]->m_im; - c.m_invwi[0][0] += m*(q[1]+q[2]); - c.m_invwi[1][1] += m*(q[0]+q[2]); - c.m_invwi[2][2] += m*(q[0]+q[1]); - c.m_invwi[0][1] -= m*k[0]*k[1]; - c.m_invwi[0][2] -= m*k[0]*k[2]; - c.m_invwi[1][2] -= m*k[1]*k[2]; - } - c.m_invwi[1][0]=c.m_invwi[0][1]; - c.m_invwi[2][0]=c.m_invwi[0][2]; - c.m_invwi[2][1]=c.m_invwi[1][2]; - c.m_invwi=c.m_invwi.inverse(); -#endif -#endif - /* Velocities */ - c.m_lv=btVector3(0,0,0); - c.m_av=btVector3(0,0,0); - { - int i; - - for(i=0;im_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); - } - } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) - { - for(int j=0;jm_x; - btVector3 mx=mi; - for(int j=1;jm_x); - mx.setMax(c.m_nodes[j]->m_x); - } - ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); - else - c.m_leaf=m_cdbvt.insert(bounds,&c); - } - } - } -} - -// -void btSoftBody::cleanupClusters() -{ - for(int i=0;iTerminate(m_sst.sdt); - if(m_joints[i]->m_delete) - { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); - } - } -} - -// -void btSoftBody::prepareClusters(int iterations) -{ - for(int i=0;iPrepare(m_sst.sdt,iterations); - } -} - - -// -void btSoftBody::solveClusters(btScalar sor) -{ - for(int i=0,ni=m_joints.size();iSolve(m_sst.sdt,sor); - } -} - -// -void btSoftBody::applyClusters(bool drift) -{ - BT_PROFILE("ApplyClusters"); - const btScalar f0=m_sst.sdt; - const btScalar f1=f0/2; - btAlignedObjectArray deltas; - btAlignedObjectArray weights; - deltas.resize(m_nodes.size(),btVector3(0,0,0)); - weights.resize(m_nodes.size(),0); - int i; - - if(drift) - { - for(i=0;im_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+cross(w,x-c.m_com))*q; - weights[idx] += q; - } - } - } - for(i=0;i0) m_nodes[i].m_x+=deltas[i]/weights[i]; - } -} - -// -void btSoftBody::dampClusters() -{ - int i; - - for(i=0;i0) - { - for(int j=0;j0) - { - const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - if(vx.length2()<=n.m_v.length2()) - { - n.m_v += c.m_ndamping*(vx-n.m_v); - } - } - } - } - } -} - -// -void btSoftBody::Joint::Prepare(btScalar dt,int) -{ - m_bodies[0].activate(); - m_bodies[1].activate(); -} - -// -void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) -{ - static const btScalar maxdrift=4; - Joint::Prepare(dt,iterations); - m_rpos[0] = m_bodies[0].xform()*m_refs[0]; - m_rpos[1] = m_bodies[1].xform()*m_refs[1]; - m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; - m_rpos[0] -= m_bodies[0].xform().getOrigin(); - m_rpos[1] -= m_bodies[1].xform().getOrigin(); - m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], - m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); - if(m_split>0) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift /=(btScalar)iterations; -} - -// -void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vr=va-vb; - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); -} - -// -void btSoftBody::LJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); - } -} - -// -void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) -{ - static const btScalar maxdrift=SIMD_PI/16; - m_icontrol->Prepare(this); - Joint::Prepare(dt,iterations); - m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; - m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; - m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); - m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); - m_drift *= m_erp/dt; - m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); - if(m_split>0) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift /=(btScalar)iterations; -} - -// -void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].angularVelocity(); - const btVector3 vb=m_bodies[1].angularVelocity(); - const btVector3 vr=va-vb; - const btScalar sp=dot(vr,m_axis[0]); - const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; - m_bodies[0].applyAImpulse(-impulse); - m_bodies[1].applyAImpulse( impulse); -} - -// -void btSoftBody::AJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); - } -} - -// -void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) -{ - Joint::Prepare(dt,iterations); - const bool dodrift=(m_life==0); - m_delete=(++m_life)>m_maxlife; - if(dodrift) - { - m_drift=m_drift*m_erp/dt; - if(m_split>0) - { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; - } - m_drift/=(btScalar)iterations; - } - else - { - m_drift=m_sdrift=btVector3(0,0,0); - } -} - -// -void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) -{ - const btVector3 va=m_bodies[0].velocity(m_rpos[0]); - const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); - const btVector3 vrel=va-vb; - const btScalar rvac=dot(vrel,m_normal); - btSoftBody::Impulse impulse; - impulse.m_asVelocity = 1; - impulse.m_velocity = m_drift; - if(rvac<0) - { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; - } - impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; - m_bodies[0].applyImpulse(-impulse,m_rpos[0]); - m_bodies[1].applyImpulse( impulse,m_rpos[1]); -} - -// -void btSoftBody::CJoint::Terminate(btScalar dt) -{ - if(m_split>0) - { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); - } -} - -// -void btSoftBody::applyForces() -{ - BT_PROFILE("SoftBody applyForces"); - const btScalar dt=m_sst.sdt; - const btScalar kLF=m_cfg.kLF; - const btScalar kDG=m_cfg.kDG; - const btScalar kPR=m_cfg.kPR; - const btScalar kVC=m_cfg.kVC; - const bool as_lift=kLF>0; - const bool as_drag=kDG>0; - const bool as_pressure=kPR!=0; - const bool as_volume=kVC>0; - const bool as_aero= as_lift || - as_drag ; - const bool as_vaero= as_aero && - (m_cfg.aeromodel=btSoftBody::eAeroModel::F_TwoSided); - const bool use_medium= as_aero; - const bool use_volume= as_pressure || - as_volume ; - btScalar volume=0; - btScalar ivolumetp=0; - btScalar dvolumetv=0; - btSoftBody::sMedium medium; - if(use_volume) - { - volume = getVolume(); - ivolumetp = 1/btFabs(volume)*kPR; - dvolumetv = (m_pose.m_volume-volume)*kVC; - } - /* Per vertex forces */ - int i,ni; - - for(i=0,ni=m_nodes.size();i0) - { - if(use_medium) - { - EvaluateMedium(m_worldInfo,n.m_x,medium); - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v=n.m_v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - btVector3 nrm=n.m_n; - /* Setup normal */ - switch(m_cfg.aeromodel) - { - case btSoftBody::eAeroModel::V_Point: - nrm=NormalizeAny(rel_v);break; - case btSoftBody::eAeroModel::V_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; - } - const btScalar dvn=dot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area*dvn*rel_v2/2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - ApplyClampedForce(n,force,dt); - } - } - } - } - /* Pressure */ - if(as_pressure) - { - n.m_f += n.m_n*(n.m_area*ivolumetp); - } - /* Volume */ - if(as_volume) - { - n.m_f += n.m_n*(n.m_area*dvolumetv); - } - } - } - /* Per face forces */ - for(i=0,ni=m_faces.size();im_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(m_worldInfo,x,medium); - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - btVector3 nrm=f.m_normal; - /* Setup normal */ - switch(m_cfg.aeromodel) - { - case btSoftBody::eAeroModel::F_TwoSided: - nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; - } - const btScalar dvn=dot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); - } - } - } - } -} - -// -void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) -{ - const btScalar kAHR=psb->m_cfg.kAHR*kst; - const btScalar dt=psb->m_sst.sdt; - for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; - const btTransform& t=a.m_body->getInterpolationWorldTransform(); - Node& n=*a.m_node; - const btVector3 wa=t*a.m_local; - const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr; - n.m_x+=impulse*a.m_c2; - a.m_body->applyImpulse(-impulse,a.m_c1); - } -} - -// -void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) -{ - const btScalar dt=psb->m_sst.sdt; - const btScalar mrg=psb->getCollisionShape()->getMargin(); - for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; - const sCti& cti=c.m_cti; - btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); - - const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb=c.m_node->m_x-c.m_node->m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,cti.m_normal); - if(dn<=SIMD_EPSILON) - { - const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); - const btVector3 fv=vr-cti.m_normal*dn; - const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); - c.m_node->m_x-=impulse*c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); - } - } -} - -// -void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) -{ - for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - if(dot(vr,nr)<0) - { - const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); - corr+=c.m_normal*j; - } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); - } -} - -// -void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) -{ - for(int i=0,ni=psb->m_links.size();im_links[i]; - if(l.m_c0>0) - { - Node& a=*l.m_n[0]; - Node& b=*l.m_n[1]; - const btVector3 del=b.m_x-a.m_x; - const btScalar len=del.length2(); - const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; - const btScalar t=k*a.m_im; - a.m_x-=del*(k*a.m_im); - b.m_x+=del*(k*b.m_im); - } - } -} - -// -void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) -{ - for(int i=0,ni=psb->m_links.size();im_links[i]; - Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); - } -} - -// -btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) -{ - switch(solver) - { - case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); - case ePSolver::Linear: return(&btSoftBody::PSolve_Links); - case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); - case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); - } - return(0); -} - -// -btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) -{ - switch(solver) - { - case eVSolver::Linear: return(&btSoftBody::VSolve_Links); - } - return(0); -} - -// -void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) -{ - switch(m_cfg.collisions&fCollision::RVSmask) - { - case fCollision::SDF_RS: - { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb1=btRigidBody::upcast(pco); - btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); - - const btTransform ctr=pco->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; - docollide.m_colObj1 = pco; - docollide.m_rigidBody = prb1; - - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); - } - break; - case fCollision::CL_RS: - { - btSoftColliders::CollideCL_RS collider; - collider.Process(this,pco); - } - break; - } -} - -// -void btSoftBody::defaultCollisionHandler(btSoftBody* psb) -{ - const int cf=m_cfg.collisions&psb->m_cfg.collisions; - switch(cf&fCollision::SVSmask) - { - case fCollision::CL_SS: - { - btSoftColliders::CollideCL_SS docollide; - docollide.Process(this,psb); - } - break; - case fCollision::VF_SS: - { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - break; - } -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +#include "btSoftBodyInternals.h" + +// +btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) +:m_worldInfo(worldInfo) +{ + /* Init */ + m_internalType = CO_SOFT_BODY; + m_cfg.aeromodel = eAeroModel::V_Point; + m_cfg.kVCF = 1; + m_cfg.kDG = 0; + m_cfg.kLF = 0; + m_cfg.kDP = 0; + m_cfg.kPR = 0; + m_cfg.kVC = 0; + m_cfg.kDF = (btScalar)0.2; + m_cfg.kMT = 0; + m_cfg.kCHR = (btScalar)1.0; + m_cfg.kKHR = (btScalar)0.1; + m_cfg.kSHR = (btScalar)1.0; + m_cfg.kAHR = (btScalar)0.7; + m_cfg.kSRHR_CL = (btScalar)0.1; + m_cfg.kSKHR_CL = (btScalar)1; + m_cfg.kSSHR_CL = (btScalar)0.5; + m_cfg.kSR_SPLT_CL = (btScalar)0.5; + m_cfg.kSK_SPLT_CL = (btScalar)0.5; + m_cfg.kSS_SPLT_CL = (btScalar)0.5; + m_cfg.maxvolume = (btScalar)1; + m_cfg.timescale = 1; + m_cfg.viterations = 0; + m_cfg.piterations = 1; + m_cfg.diterations = 0; + m_cfg.citerations = 4; + m_cfg.collisions = fCollision::Default; + m_pose.m_bvolume = false; + m_pose.m_bframe = false; + m_pose.m_volume = 0; + m_pose.m_com = btVector3(0,0,0); + m_pose.m_rot.setIdentity(); + m_pose.m_scl.setIdentity(); + m_tag = 0; + m_timeacc = 0; + m_bUpdateRtCst = true; + m_bounds[0] = btVector3(0,0,0); + m_bounds[1] = btVector3(0,0,0); + m_worldTransform.setIdentity(); + setSolver(eSolverPresets::Positions); + /* Default material */ + Material* pm=appendMaterial(); + pm->m_kLST = 1; + pm->m_kAST = 1; + pm->m_kVST = 1; + pm->m_flags = fMaterial::Default; + /* Collision shape */ + ///for now, create a collision shape internally + m_collisionShape = new btSoftBodyCollisionShape(this); + m_collisionShape->setMargin(0.25); + /* Nodes */ + const btScalar margin=getCollisionShape()->getMargin(); + m_nodes.resize(node_count); + for(int i=0,ni=node_count;i0?1/n.m_im:0; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + n.m_material= pm; + } + updateBounds(); + + +} + +// +btSoftBody::~btSoftBody() +{ + //for now, delete the internal shape + delete m_collisionShape; + int i; + + releaseClusters(); + for(i=0;i0) + *pm=*m_materials[0]; + else + ZeroInitialize(*pm); + m_materials.push_back(pm); + return(pm); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) +{ + Note n; + ZeroInitialize(n); + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0;n.m_rank+=n0?1:0; + n.m_nodes[1] = n1;n.m_rank+=n1?1:0; + n.m_nodes[2] = n2;n.m_rank+=n2?1:0; + n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + m_notes.push_back(n); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Node* feature) +{ + appendNote(text,o,btVector4(1,0,0,0),feature); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Link* feature) +{ + static const btScalar w=1/(btScalar)2; + appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], + feature->m_n[1]); +} + +// +void btSoftBody::appendNote( const char* text, + const btVector3& o, + Face* feature) +{ + static const btScalar w=1/(btScalar)3; + appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); +} + +// +void btSoftBody::appendNode( const btVector3& x,btScalar m) +{ + if(m_nodes.capacity()==m_nodes.size()) + { + pointersToIndices(); + m_nodes.reserve(m_nodes.size()*2+1); + indicesToPointers(); + } + const btScalar margin=getCollisionShape()->getMargin(); + m_nodes.push_back(Node()); + Node& n=m_nodes[m_nodes.size()-1]; + ZeroInitialize(n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m>0?1/m:0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); +} + +// +void btSoftBody::appendLink(int model,Material* mat) +{ + Link l; + if(model>=0) + l=m_links[model]; + else + { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } + m_links.push_back(l); +} + +// +void btSoftBody::appendLink( int node0, + int node1, + Material* mat, + bool bcheckexist) +{ + appendLink(&m_nodes[node0],&m_nodes[node1],mat,bcheckexist); +} + +// +void btSoftBody::appendLink( Node* node0, + Node* node1, + Material* mat, + bool bcheckexist) +{ + if((!bcheckexist)||(!checkLink(node0,node1))) + { + appendLink(-1,mat); + Link& l=m_links[m_links.size()-1]; + l.m_n[0] = node0; + l.m_n[1] = node1; + l.m_rl = (l.m_n[0]->m_x-l.m_n[1]->m_x).length(); + m_bUpdateRtCst=true; + } +} + +// +void btSoftBody::appendFace(int model,Material* mat) +{ + Face f; + if(model>=0) + { f=m_faces[model]; } + else + { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } + m_faces.push_back(f); +} + +// +void btSoftBody::appendFace(int node0,int node1,int node2,Material* mat) +{ + if (node0==node1) + return; + if (node1==node2) + return; + if (node2==node0) + return; + + appendFace(-1,mat); + Face& f=m_faces[m_faces.size()-1]; + btAssert(node0!=node1); + btAssert(node1!=node2); + btAssert(node2!=node0); + f.m_n[0] = &m_nodes[node0]; + f.m_n[1] = &m_nodes[node1]; + f.m_n[2] = &m_nodes[node2]; + f.m_ra = AreaOf( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + m_bUpdateRtCst=true; +} + +// +void btSoftBody::appendAnchor(int node,btRigidBody* body) +{ + Anchor a; + a.m_node = &m_nodes[node]; + a.m_body = body; + a.m_local = body->getInterpolationWorldTransform().inverse()*a.m_node->m_x; + a.m_node->m_battach = 1; + m_anchors.push_back(a); +} + +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) +{ + LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + m_joints.push_back(pj); +} + +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) +{ + appendLinearJoint(specs,m_clusters[0],body); +} + +// +void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) +{ + appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) +{ + AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; + m_joints.push_back(pj); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) +{ + appendAngularJoint(specs,m_clusters[0],body); +} + +// +void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) +{ + appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); +} + +// +void btSoftBody::addForce(const btVector3& force) +{ + for(int i=0,ni=m_nodes.size();i0) + { + n.m_f += force; + } +} + +// +void btSoftBody::addVelocity(const btVector3& velocity) +{ + for(int i=0,ni=m_nodes.size();i0) + { + n.m_v = velocity; + } + } +} + + +// +void btSoftBody::addVelocity(const btVector3& velocity,int node) +{ + Node& n=m_nodes[node]; + if(n.m_im>0) + { + n.m_v += velocity; + } +} + +// +void btSoftBody::setMass(int node,btScalar mass) +{ + m_nodes[node].m_im=mass>0?1/mass:0; + m_bUpdateRtCst=true; +} + +// +btScalar btSoftBody::getMass(int node) const +{ + return(m_nodes[node].m_im>0?1/m_nodes[node].m_im:0); +} + +// +btScalar btSoftBody::getTotalMass() const +{ + btScalar mass=0; + for(int i=0;im_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x); + for(int j=0;j<3;++j) + { + f.m_n[j]->m_im+=twicearea; + } + } + for( i=0;igetMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + + for(int i=0,ni=m_nodes.size();igetMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; + + for(int i=0,ni=m_nodes.size();i0 ? + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; + } + /* Pos */ + const btVector3 com=evaluateCom(); + m_pose.m_pos.resize(m_nodes.size()); + for( i=0,ni=m_nodes.size();i0) + { + int i,ni; + + const btVector3 org=m_nodes[0].m_x; + for(i=0,ni=m_faces.size();im_x-org,cross(f.m_n[1]->m_x-org,f.m_n[2]->m_x-org)); + } + vol/=(btScalar)6; + } + return(vol); +} + +// +int btSoftBody::clusterCount() const +{ + return(m_clusters.size()); +} + +// +btVector3 btSoftBody::clusterCom(const Cluster* cluster) +{ + btVector3 com(0,0,0); + for(int i=0,ni=cluster->m_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; + } + return(com*cluster->m_imass); +} + +// +btVector3 btSoftBody::clusterCom(int cluster) const +{ + return(clusterCom(m_clusters[cluster])); +} + +// +btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) +{ + return(cluster->m_lv+cross(cluster->m_av,rpos)); +} + +// +void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +{ + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; +} + +// +void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) +{ + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_dimpulses[0]+=li; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; +} + +// +void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) +{ + if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); + if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); +} + +// +void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) +{ + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; +} + +// +void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) +{ + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; +} + +// +void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) +{ + if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); + if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); +} + +// +void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) +{ + cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_ndimpulses++; +} + +// +int btSoftBody::generateBendingConstraints(int distance,Material* mat) +{ + int i,j; + + if(distance>1) + { + /* Build graph */ + const int n=m_nodes.size(); + const unsigned inf=(~(unsigned)0)>>1; + unsigned* adj=new unsigned[n*n]; +#define IDX(_x_,_y_) ((_y_)*n+(_x_)) + for(j=0;jsum) + { + adj[IDX(i,j)]=adj[IDX(j,i)]=sum; + } + } + } + } + /* Build links */ + int nlinks=0; + for(j=0;jm_leaf) m_cdbvt.remove(c->m_leaf); + c->~Cluster(); + btAlignedFree(c); + m_clusters.remove(c); +} + +// +void btSoftBody::releaseClusters() +{ + while(m_clusters.size()>0) releaseCluster(0); +} + +// +int btSoftBody::generateClusters(int k,int maxiterations) +{ + int i; + releaseClusters(); + m_clusters.resize(btMin(k,m_nodes.size())); + for(i=0;im_collide= true; + } + k=m_clusters.size(); + if(k>0) + { + /* Initialize */ + btAlignedObjectArray centers; + btVector3 cog(0,0,0); + int i; + for(i=0;im_nodes.push_back(&m_nodes[i]); + } + cog/=(btScalar)m_nodes.size(); + centers.resize(k,cog); + /* Iterate */ + const btScalar slope=16; + bool changed; + int iterations=0; + do { + const btScalar w=2-btMin(1,iterations/slope); + changed=false; + iterations++; + int i; + + for(i=0;im_nodes.size();++j) + { + c+=m_clusters[i]->m_nodes[j]->m_x; + } + if(m_clusters[i]->m_nodes.size()) + { + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i]+(c-centers[i])*w; + changed |= ((c-centers[i]).length2()>SIMD_EPSILON); + centers[i] = c; + m_clusters[i]->m_nodes.resize(0); + } + } + for(i=0;im_nodes.push_back(&m_nodes[i]); + } + } while(changed&&(iterations cids; + cids.resize(m_nodes.size(),-1); + for(i=0;im_nodes.size();++j) + { + cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + } + } + for(i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + { + m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); + } + } + } + } + } + /* Master */ + if(m_clusters.size()>1) + { + Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + pmaster->m_collide = false; + pmaster->m_nodes.reserve(m_nodes.size()); + for(int i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters.push_back(pmaster); + btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + } + /* Terminate */ + for(i=0;im_nodes.size()==0) + { + releaseCluster(i--); + } + } + + initializeClusters(); + updateClusters(); + return(m_clusters.size()); + } + return(0); +} + +// +void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) +{ + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix edges(ncount,-2); + int newnodes=0; + int i,j,k,ni; + + /* Filter out */ + for(i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + { + btSwap(m_links[i],m_links[m_links.size()-1]); + m_links.pop_back();--i; + } + } + } + /* Fill edges */ + for(i=0;i0) + { + const btVector3 x=Lerp(a.m_x,b.m_x,t); + const btVector3 v=Lerp(a.m_v,b.m_v,t); + btScalar m=0; + if(a.m_im>0) + { + if(b.m_im>0) + { + const btScalar ma=1/a.m_im; + const btScalar mb=1/b.m_im; + const btScalar mc=Lerp(ma,mb,t); + const btScalar f=(ma+mb)/(ma+mb+mc); + a.m_im=1/(ma*f); + b.m_im=1/(mb*f); + m=mc*f; + } + else + { a.m_im/=0.5;m=1/a.m_im; } + } + else + { + if(b.m_im>0) + { b.m_im/=0.5;m=1/b.m_im; } + else + m=0; + } + appendNode(x,m); + edges(i,j)=m_nodes.size()-1; + m_nodes[edges(i,j)].m_v=v; + ++newnodes; + } + } + } + } + nbase=&m_nodes[0]; + /* Refine links */ + for(i=0,ni=m_links.size();i0) + { + appendLink(i); + Link* pft[]={ &m_links[i], + &m_links[m_links.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[0]]; + pft[0]->m_n[1]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[1]]; + } + } + } + /* Refine faces */ + for(i=0;i0) + { + appendFace(i); + const int l=(k+1)%3; + Face* pft[]={ &m_faces[i], + &m_faces[m_faces.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[l]]; + pft[0]->m_n[1]=&m_nodes[idx[j]]; + pft[0]->m_n[2]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[k]]; + pft[1]->m_n[2]=&m_nodes[idx[l]]; + appendLink(ni,idx[l],pft[0]->m_material); + --i;break; + } + } + } + } + /* Cut */ + if(cut) + { + btAlignedObjectArray cnodes; + const int pcount=ncount; + int i; + ncount=m_nodes.size(); + cnodes.resize(ncount,0); + /* Nodes */ + for(i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } + appendNode(x,m); + cnodes[i]=m_nodes.size()-1; + m_nodes[cnodes[i]].m_v=v; + } + } + nbase=&m_nodes[0]; + /* Links */ + for(i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; + btAlignedObjectArray todelete; + ranks.resize(nnodes,0); + for(i=0,ni=m_links.size();i=0;--i) + { + if(!ranks[i]) todelete.push_back(i); + } + if(todelete.size()) + { + btAlignedObjectArray& map=ranks; + for(int i=0;im_v=v; + pn[1]->m_v=v; + for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; + pft[1]->m_n[0]=pn[1-mtch]; + done=true; + } + } + for(i=0,ni=m_faces.size();im_n[l]=pn[mtch]; + pft[1]->m_n[k]=pn[1-mtch]; + appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + } + } + } + if(!done) + { + m_ndbvt.remove(pn[0]->m_leaf); + m_ndbvt.remove(pn[1]->m_leaf); + m_nodes.pop_back(); + m_nodes.pop_back(); + } + return(done); +} + +// +bool btSoftBody::rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results) +{ + if(m_faces.size()&&m_fdbvt.empty()) + initializeFaceTree(); + + results.body = this; + results.fraction = 1.f; + results.feature = eFeature::None; + results.index = -1; + + return(rayTest(rayFrom,rayTo,results.fraction,results.feature,results.index,false)!=0); +} + +// +void btSoftBody::setSolver(eSolverPresets::_ preset) +{ + m_cfg.m_vsequence.clear(); + m_cfg.m_psequence.clear(); + m_cfg.m_dsequence.clear(); + switch(preset) + { + case eSolverPresets::Positions: + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; + case eSolverPresets::Velocities: + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; + } +} + +// +void btSoftBody::predictMotion(btScalar dt) +{ + int i,ni; + + /* Update */ + if(m_bUpdateRtCst) + { + m_bUpdateRtCst=false; + updateConstants(); + m_fdbvt.clear(); + if(m_cfg.collisions&fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt*m_cfg.timescale; + m_sst.isdt = 1/m_sst.sdt; + m_sst.velmrg = m_sst.sdt*3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg*(btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity*m_sst.sdt); + applyForces(); + /* Integrate */ + for(i=0,ni=m_nodes.size();im_v+ + f.m_n[1]->m_v+ + f.m_n[2]->m_v)/3; + vol = VolumeOf(f,m_sst.radmrg); + m_fdbvt.update( f.m_leaf, + vol, + v*m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if(m_pose.m_bframe&&(m_cfg.kMT>0)) + { + const btMatrix3x3 posetrs=m_pose.m_rot; + for(int i=0,ni=m_nodes.size();i0) + { + const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; + n.m_x=Lerp(n.m_x,x,m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); +} + +// +void btSoftBody::solveConstraints() +{ + /* Apply clusters */ + applyClusters(false); + /* Prepare links */ + + int i,ni; + + for(i=0,ni=m_links.size();im_q-l.m_n[0]->m_q; + l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + } + /* Prepare anchors */ + for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt*a.m_node->m_im; + a.m_body->activate(); + } + /* Solve velocities */ + if(m_cfg.viterations>0) + { + /* Solve */ + for(int isolve=0;isolve0) + { + for(int isolve=0;isolve0) + { + const btScalar vcf=m_cfg.kVCF*m_sst.isdt; + for(i=0,ni=m_nodes.size();i& bodies) +{ + const int nb=bodies.size(); + int iterations=0; + int i; + + for(i=0;im_cfg.citerations); + } + for(i=0;iprepareClusters(iterations); + } + for(i=0;isolveClusters(sor); + } + } + for(i=0;icleanupClusters(); + } +} + +// +void btSoftBody::integrateMotion() +{ + /* Update */ + updateNormals(); +} + +// +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) +{ + m_rayFrom = rayFrom; + m_rayNormalizedDirection = (rayTo-rayFrom); + m_rayTo = rayTo; + m_mint = mxt; + m_face = 0; + m_tests = 0; +} + +// +void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) +{ + btSoftBody::Face& f=*(btSoftBody::Face*)leaf->data; + const btScalar t=rayFromToTriangle( m_rayFrom,m_rayTo,m_rayNormalizedDirection, + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + m_mint); + if((t>0)&&(tteps)&&(tceps) && + (dot(n,cross(b-hit,c-hit))>ceps) && + (dot(n,cross(c-hit,a-hit))>ceps)) + { + return(t); + } + } + } + return(-1); +} + +// +void btSoftBody::pointersToIndices() +{ +#define PTR2IDX(_p_,_b_) reinterpret_cast((_p_)-(_b_)) + btSoftBody::Node* base=&m_nodes[0]; + int i,ni; + + for(i=0,ni=m_nodes.size();idata=*(void**)&i; + } + } + for(i=0,ni=m_links.size();idata=*(void**)&i; + } + } + for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; + } + } + for(i=0,ni=m_links.size();idata=&m_faces[i]; + } + } + for(i=0,ni=m_anchors.size();im_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); + if(t>0) + { + ++cnt; + if(!bcountonly) + { + feature=btSoftBody::eFeature::Face; + index=i; + mint=t; + } + } + } + } + else + {/* Use dbvt */ + RayFromToCaster collider(rayFrom,rayTo,mint); + + btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); + if(collider.m_face) + { + mint=collider.m_mint; + feature=btSoftBody::eFeature::Face; + index=(int)(collider.m_face-&m_faces[0]); + cnt=1; + } + } + return(cnt); +} + +// +void btSoftBody::initializeFaceTree() +{ + m_fdbvt.clear(); + for(int i=0;igetCollisionShape(); + btRigidBody* tmpRigid = btRigidBody::upcast(colObj); + const btTransform& wtr=tmpRigid? tmpRigid->getInterpolationWorldTransform() : colObj->getWorldTransform(); + btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), + shp, + nrm, + margin); + if(dst<0) + { + cti.m_colObj = colObj; + cti.m_normal = wtr.getBasis()*nrm; + cti.m_offset = -dot( cti.m_normal, + x-cti.m_normal*dst); + return(true); + } + return(false); +} + +// +void btSoftBody::updateNormals() +{ + const btVector3 zv(0,0,0); + int i,ni; + + for(i=0,ni=m_nodes.size();im_x-f.m_n[0]->m_x, + f.m_n[2]->m_x-f.m_n[0]->m_x); + f.m_normal=n.normalized(); + f.m_n[0]->m_n+=n; + f.m_n[1]->m_n+=n; + f.m_n[2]->m_n+=n; + } + for(i=0,ni=m_nodes.size();iSIMD_EPSILON) + m_nodes[i].m_n /= len; + } +} + +// +void btSoftBody::updateBounds() +{ + if(m_ndbvt.m_root) + { + const btVector3& mins=m_ndbvt.m_root->volume.Mins(); + const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); + const btScalar csm=getCollisionShape()->getMargin(); + const btVector3 mrg=btVector3( csm, + csm, + csm)*1; // ??? to investigate... + m_bounds[0]=mins-mrg; + m_bounds[1]=maxs+mrg; + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0]= + m_bounds[1]=btVector3(0,0,0); + } +} + + +// +void btSoftBody::updatePose() +{ + if(m_pose.m_bframe) + { + btSoftBody::Pose& pose=m_pose; + const btVector3 com=evaluateCom(); + /* Com */ + pose.m_com = com; + /* Rotation */ + btMatrix3x3 Apq; + const btScalar eps=SIMD_EPSILON; + Apq[0]=Apq[1]=Apq[2]=btVector3(0,0,0); + Apq[0].setX(eps);Apq[1].setY(eps*2);Apq[2].setZ(eps*3); + for(int i=0,ni=m_nodes.size();i1) + { + const btScalar idet=Clamp( 1/pose.m_scl.determinant(), + 1,m_cfg.maxvolume); + pose.m_scl=Mul(pose.m_scl,idet); + } + + } +} + +// +void btSoftBody::updateConstants() +{ + int i,ni; + + /* Links */ + for(i=0,ni=m_links.size();im_x-l.m_n[1]->m_x).length(); + l.m_c0 = (l.m_n[0]->m_im+l.m_n[1]->m_im)/m.m_kLST; + l.m_c1 = l.m_rl*l.m_rl; + } + /* Faces */ + for(i=0,ni=m_faces.size();im_x,f.m_n[1]->m_x,f.m_n[2]->m_x); + } + /* Area's */ + btAlignedObjectArray counts; + counts.resize(m_nodes.size(),0); + for(i=0,ni=m_nodes.size();im_area+=btFabs(f.m_ra); + } + } + for(i=0,ni=m_nodes.size();i0) + m_nodes[i].m_area/=(btScalar)counts[i]; + else + m_nodes[i].m_area=0; + } +} + +// +void btSoftBody::initializeClusters() +{ + int i; + + for( i=0;im_im>0?1/c.m_nodes[j]->m_im:0; + c.m_imass += c.m_masses[j]; + } + c.m_imass = 1/c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0,0,0); + c.m_av = btVector3(0,0,0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii=c.m_locii; + ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + { + int i,ni; + + for(i=0,ni=c.m_nodes.size();im_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=c.m_masses[i]; + ii[0][0] += m*(q[1]+q[2]); + ii[1][1] += m*(q[0]+q[2]); + ii[2][2] += m*(q[0]+q[1]); + ii[0][1] -= m*k[0]*k[1]; + ii[0][2] -= m*k[0]*k[2]; + ii[1][2] -= m*k[1]*k[2]; + } + } + ii[1][0]=ii[0][1]; + ii[2][0]=ii[0][2]; + ii[2][1]=ii[1][2]; + ii=ii.inverse(); + /* Frame */ + c.m_framexform.setIdentity(); + c.m_framexform.setOrigin(c.m_com); + c.m_framerefs.resize(c.m_nodes.size()); + { + int i; + for(i=0;im_x-c.m_com; + } + } + } +} + +// +void btSoftBody::updateClusters() +{ + BT_PROFILE("UpdateClusters"); + int i; + + for(i=0;im_x-c.m_com; + const btVector3& b=c.m_framerefs[i]; + m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; + } + PolarDecompose(m,r,s); + c.m_framexform.setOrigin(c.m_com); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1/* Constant */ + c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); +#else +#if 0/* Sphere */ + const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); + const btVector3 inertia(rk,rk,rk); + const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, + btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, + btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); + + c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); +#else/* Actual */ + c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); + for(int i=0;im_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=1/c.m_nodes[i]->m_im; + c.m_invwi[0][0] += m*(q[1]+q[2]); + c.m_invwi[1][1] += m*(q[0]+q[2]); + c.m_invwi[2][2] += m*(q[0]+q[1]); + c.m_invwi[0][1] -= m*k[0]*k[1]; + c.m_invwi[0][2] -= m*k[0]*k[2]; + c.m_invwi[1][2] -= m*k[1]*k[2]; + } + c.m_invwi[1][0]=c.m_invwi[0][1]; + c.m_invwi[2][0]=c.m_invwi[0][2]; + c.m_invwi[2][1]=c.m_invwi[1][2]; + c.m_invwi=c.m_invwi.inverse(); +#endif +#endif + /* Velocities */ + c.m_lv=btVector3(0,0,0); + c.m_av=btVector3(0,0,0); + { + int i; + + for(i=0;im_v*c.m_masses[i]; + c.m_lv += v; + c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); + } + } + c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); + c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0,0,0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0,0,0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if(c.m_matching>0) + { + for(int j=0;jm_x; + btVector3 mx=mi; + for(int j=1;jm_x); + mx.setMax(c.m_nodes[j]->m_x); + } + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); + if(c.m_leaf) + m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + else + c.m_leaf=m_cdbvt.insert(bounds,&c); + } + } + } +} + +// +void btSoftBody::cleanupClusters() +{ + for(int i=0;iTerminate(m_sst.sdt); + if(m_joints[i]->m_delete) + { + btAlignedFree(m_joints[i]); + m_joints.remove(m_joints[i--]); + } + } +} + +// +void btSoftBody::prepareClusters(int iterations) +{ + for(int i=0;iPrepare(m_sst.sdt,iterations); + } +} + + +// +void btSoftBody::solveClusters(btScalar sor) +{ + for(int i=0,ni=m_joints.size();iSolve(m_sst.sdt,sor); + } +} + +// +void btSoftBody::applyClusters(bool drift) +{ + BT_PROFILE("ApplyClusters"); + const btScalar f0=m_sst.sdt; + const btScalar f1=f0/2; + btAlignedObjectArray deltas; + btAlignedObjectArray weights; + deltas.resize(m_nodes.size(),btVector3(0,0,0)); + weights.resize(m_nodes.size(),0); + int i; + + if(drift) + { + for(i=0;im_x; + const btScalar q=c.m_masses[j]; + deltas[idx] += (v+cross(w,x-c.m_com))*q; + weights[idx] += q; + } + } + } + for(i=0;i0) m_nodes[i].m_x+=deltas[i]/weights[i]; + } +} + +// +void btSoftBody::dampClusters() +{ + int i; + + for(i=0;i0) + { + for(int j=0;j0) + { + const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + if(vx.length2()<=n.m_v.length2()) + { + n.m_v += c.m_ndamping*(vx-n.m_v); + } + } + } + } + } +} + +// +void btSoftBody::Joint::Prepare(btScalar dt,int) +{ + m_bodies[0].activate(); + m_bodies[1].activate(); +} + +// +void btSoftBody::LJoint::Prepare(btScalar dt,int iterations) +{ + static const btScalar maxdrift=4; + Joint::Prepare(dt,iterations); + m_rpos[0] = m_bodies[0].xform()*m_refs[0]; + m_rpos[1] = m_bodies[1].xform()*m_refs[1]; + m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], + m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); + if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } + m_drift /=(btScalar)iterations; +} + +// +void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) +{ + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr=va-vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); +} + +// +void btSoftBody::LJoint::Terminate(btScalar dt) +{ + if(m_split>0) + { + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + } +} + +// +void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) +{ + static const btScalar maxdrift=SIMD_PI/16; + m_icontrol->Prepare(this); + Joint::Prepare(dt,iterations); + m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; + m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); + m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); + m_drift *= m_erp/dt; + m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); + if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } + m_drift /=(btScalar)iterations; +} + +// +void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) +{ + const btVector3 va=m_bodies[0].angularVelocity(); + const btVector3 vb=m_bodies[1].angularVelocity(); + const btVector3 vr=va-vb; + const btScalar sp=dot(vr,m_axis[0]); + const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + m_bodies[0].applyAImpulse(-impulse); + m_bodies[1].applyAImpulse( impulse); +} + +// +void btSoftBody::AJoint::Terminate(btScalar dt) +{ + if(m_split>0) + { + m_bodies[0].applyDAImpulse(-m_sdrift); + m_bodies[1].applyDAImpulse( m_sdrift); + } +} + +// +void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) +{ + Joint::Prepare(dt,iterations); + const bool dodrift=(m_life==0); + m_delete=(++m_life)>m_maxlife; + if(dodrift) + { + m_drift=m_drift*m_erp/dt; + if(m_split>0) + { + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; + } + m_drift/=(btScalar)iterations; + } + else + { + m_drift=m_sdrift=btVector3(0,0,0); + } +} + +// +void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) +{ + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel=va-vb; + const btScalar rvac=dot(vrel,m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if(rvac<0) + { + const btVector3 iv=m_normal*rvac; + const btVector3 fv=vrel-iv; + impulse.m_velocity += iv+fv*m_friction; + } + impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); +} + +// +void btSoftBody::CJoint::Terminate(btScalar dt) +{ + if(m_split>0) + { + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + } +} + +// +void btSoftBody::applyForces() +{ + BT_PROFILE("SoftBody applyForces"); + const btScalar dt=m_sst.sdt; + const btScalar kLF=m_cfg.kLF; + const btScalar kDG=m_cfg.kDG; + const btScalar kPR=m_cfg.kPR; + const btScalar kVC=m_cfg.kVC; + const bool as_lift=kLF>0; + const bool as_drag=kDG>0; + const bool as_pressure=kPR!=0; + const bool as_volume=kVC>0; + const bool as_aero= as_lift || + as_drag ; + const bool as_vaero= as_aero && + (m_cfg.aeromodel=btSoftBody::eAeroModel::F_TwoSided); + const bool use_medium= as_aero; + const bool use_volume= as_pressure || + as_volume ; + btScalar volume=0; + btScalar ivolumetp=0; + btScalar dvolumetv=0; + btSoftBody::sMedium medium; + if(use_volume) + { + volume = getVolume(); + ivolumetp = 1/btFabs(volume)*kPR; + dvolumetv = (m_pose.m_volume-volume)*kVC; + } + /* Per vertex forces */ + int i,ni; + + for(i=0,ni=m_nodes.size();i0) + { + if(use_medium) + { + EvaluateMedium(m_worldInfo,n.m_x,medium); + /* Aerodynamics */ + if(as_vaero) + { + const btVector3 rel_v=n.m_v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) + { + btVector3 nrm=n.m_n; + /* Setup normal */ + switch(m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::V_Point: + nrm=NormalizeAny(rel_v);break; + case btSoftBody::eAeroModel::V_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = n.m_area*dvn*rel_v2/2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + ApplyClampedForce(n,force,dt); + } + } + } + } + /* Pressure */ + if(as_pressure) + { + n.m_f += n.m_n*(n.m_area*ivolumetp); + } + /* Volume */ + if(as_volume) + { + n.m_f += n.m_n*(n.m_area*dvolumetv); + } + } + } + /* Per face forces */ + for(i=0,ni=m_faces.size();im_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; + const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; + EvaluateMedium(m_worldInfo,x,medium); + const btVector3 rel_v=v-medium.m_velocity; + const btScalar rel_v2=rel_v.length2(); + if(rel_v2>SIMD_EPSILON) + { + btVector3 nrm=f.m_normal; + /* Setup normal */ + switch(m_cfg.aeromodel) + { + case btSoftBody::eAeroModel::F_TwoSided: + nrm*=(btScalar)(dot(nrm,rel_v)<0?-1:+1);break; + } + const btScalar dvn=dot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = f.m_ra*dvn*rel_v2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + force /= 3; + for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + } + } + } + } +} + +// +void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) +{ + const btScalar kAHR=psb->m_cfg.kAHR*kst; + const btScalar dt=psb->m_sst.sdt; + for(int i=0,ni=psb->m_anchors.size();im_anchors[i]; + const btTransform& t=a.m_body->getInterpolationWorldTransform(); + Node& n=*a.m_node; + const btVector3 wa=t*a.m_local; + const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; + const btVector3 impulse=a.m_c0*vr; + n.m_x+=impulse*a.m_c2; + a.m_body->applyImpulse(-impulse,a.m_c1); + } +} + +// +void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) +{ + const btScalar dt=psb->m_sst.sdt; + const btScalar mrg=psb->getCollisionShape()->getMargin(); + for(int i=0,ni=psb->m_rcontacts.size();im_rcontacts[i]; + const sCti& cti=c.m_cti; + btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); + + const btVector3 va=tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); + const btVector3 vb=c.m_node->m_x-c.m_node->m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,cti.m_normal); + if(dn<=SIMD_EPSILON) + { + const btScalar dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg); + const btVector3 fv=vr-cti.m_normal*dn; + const btVector3 impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst); + c.m_node->m_x-=impulse*c.m_c2; + if (tmpRigid) + tmpRigid->applyImpulse(impulse,c.m_c1); + } + } +} + +// +void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) +{ + for(int i=0,ni=psb->m_scontacts.size();im_scontacts[i]; + const btVector3& nr=c.m_normal; + Node& n=*c.m_node; + Face& f=*c.m_face; + const btVector3 p=BaryEval( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q=BaryEval( f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr=(n.m_x-n.m_q)-(p-q); + btVector3 corr(0,0,0); + if(dot(vr,nr)<0) + { + const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); + corr+=c.m_normal*j; + } + corr -= ProjectOnPlane(vr,nr)*c.m_friction; + n.m_x += corr*c.m_cfm[0]; + f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); + f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); + f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + } +} + +// +void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) +{ + for(int i=0,ni=psb->m_links.size();im_links[i]; + if(l.m_c0>0) + { + Node& a=*l.m_n[0]; + Node& b=*l.m_n[1]; + const btVector3 del=b.m_x-a.m_x; + const btScalar len=del.length2(); + const btScalar k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst; + const btScalar t=k*a.m_im; + a.m_x-=del*(k*a.m_im); + b.m_x+=del*(k*b.m_im); + } + } +} + +// +void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) +{ + for(int i=0,ni=psb->m_links.size();im_links[i]; + Node** n=l.m_n; + const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; + n[0]->m_v+= l.m_c3*(j*n[0]->m_im); + n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + } +} + +// +btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) +{ + switch(solver) + { + case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); + case ePSolver::Linear: return(&btSoftBody::PSolve_Links); + case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); + case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); + } + return(0); +} + +// +btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) +{ + switch(solver) + { + case eVSolver::Linear: return(&btSoftBody::VSolve_Links); + } + return(0); +} + +// +void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) +{ + switch(m_cfg.collisions&fCollision::RVSmask) + { + case fCollision::SDF_RS: + { + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb1=btRigidBody::upcast(pco); + btTransform wtr=prb1 ? prb1->getInterpolationWorldTransform() : pco->getWorldTransform(); + + const btTransform ctr=pco->getWorldTransform(); + const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); + const btScalar basemargin=getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), + mins, + maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(basemargin,basemargin,basemargin)); + docollide.psb = this; + docollide.m_colObj1 = pco; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root,volume,docollide); + } + break; + case fCollision::CL_RS: + { + btSoftColliders::CollideCL_RS collider; + collider.Process(this,pco); + } + break; + } +} + +// +void btSoftBody::defaultCollisionHandler(btSoftBody* psb) +{ + const int cf=m_cfg.collisions&psb->m_cfg.collisions; + switch(cf&fCollision::SVSmask) + { + case fCollision::CL_SS: + { + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); + } + break; + case fCollision::VF_SS: + { + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg= getCollisionShape()->getMargin()+ + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0]=this; + docollide.psb[1]=psb; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0]=psb; + docollide.psb[1]=this; + docollide.psb[0]->m_ndbvt.collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + break; + } +} diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 7954a195c..94d4bdbe7 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1,841 +1,841 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_H -#define _BT_SOFT_BODY_H - -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" - -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -#include "btSparseSDF.h" -#include "BulletCollision/BroadphaseCollision/btDbvt.h" - -class btBroadphaseInterface; -class btDispatcher; - -/* btSoftBodyWorldInfo */ -struct btSoftBodyWorldInfo -{ - btScalar air_density; - btScalar water_density; - btScalar water_offset; - btVector3 water_normal; - btBroadphaseInterface* m_broadphase; - btDispatcher* m_dispatcher; - btVector3 m_gravity; - btSparseSdf<3> m_sparsesdf; -}; - - -///The btSoftBody is an class to simulate cloth and volumetric soft bodies. -///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. -class btSoftBody : public btCollisionObject -{ -public: - // - // Enumerations - // - - ///eAeroModel - struct eAeroModel { enum _ { - V_Point, ///Vertex normals are oriented toward velocity - V_TwoSided, ///Vertex normals are fliped to match velocity - V_OneSided, ///Vertex normals are taken as it is - F_TwoSided, ///Face normals are fliped to match velocity - F_OneSided, ///Face normals are taken as it is - END - };}; - - ///eVSolver : velocities solvers - struct eVSolver { enum _ { - Linear, ///Linear solver - END - };}; - - ///ePSolver : positions solvers - struct ePSolver { enum _ { - Linear, ///Linear solver - Anchors, ///Anchor solver - RContacts, ///Rigid contacts solver - SContacts, ///Soft contacts solver - END - };}; - - ///eSolverPresets - struct eSolverPresets { enum _ { - Positions, - Velocities, - Default = Positions, - END - };}; - - ///eFeature - struct eFeature { enum _ { - None, - Node, - Link, - Face, - END - };}; - - typedef btAlignedObjectArray tVSolverArray; - typedef btAlignedObjectArray tPSolverArray; - - // - // Flags - // - - ///fCollision - struct fCollision { enum _ { - RVSmask = 0x000f, ///Rigid versus soft mask - SDF_RS = 0x0001, ///SDF based rigid vs soft - CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - - SVSmask = 0x00f0, ///Rigid versus soft mask - VF_SS = 0x0010, ///Vertex vs face soft vs soft handling - CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling - /* presets */ - Default = SDF_RS, - END - };}; - - ///fMaterial - struct fMaterial { enum _ { - DebugDraw = 0x0001, /// Enable debug draw - /* presets */ - Default = DebugDraw, - END - };}; - - // - // API Types - // - - /* sRayCast */ - struct sRayCast - { - btSoftBody* body; /// soft body - eFeature::_ feature; /// feature type - int index; /// feature index - btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) - }; - - /* ImplicitFn */ - struct ImplicitFn - { - virtual btScalar Eval(const btVector3& x)=0; - }; - - // - // Internal types - // - - typedef btAlignedObjectArray tScalarArray; - typedef btAlignedObjectArray tVector3Array; - - /* sCti is Softbody contact info */ - struct sCti - { - btCollisionObject* m_colObj; /* Rigid body */ - btVector3 m_normal; /* Outward normal */ - btScalar m_offset; /* Offset from origin */ - }; - - /* sMedium */ - struct sMedium - { - btVector3 m_velocity; /* Velocity */ - btScalar m_pressure; /* Pressure */ - btScalar m_density; /* Density */ - }; - - /* Base type */ - struct Element - { - void* m_tag; // User data - Element() : m_tag(0) {} - }; - /* Material */ - struct Material : Element - { - btScalar m_kLST; // Linear stiffness coefficient [0,1] - btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] - btScalar m_kVST; // Volume stiffness coefficient [0,1] - int m_flags; // Flags - }; - - /* Feature */ - struct Feature : Element - { - Material* m_material; // Material - }; - /* Node */ - struct Node : Feature - { - btVector3 m_x; // Position - btVector3 m_q; // Previous step position - btVector3 m_v; // Velocity - btVector3 m_f; // Force accumulator - btVector3 m_n; // Normal - btScalar m_im; // 1/mass - btScalar m_area; // Area - btDbvtNode* m_leaf; // Leaf data - int m_battach:1; // Attached - }; - /* Link */ - struct Link : Feature - { - Node* m_n[2]; // Node pointers - btScalar m_rl; // Rest length - int m_bbending:1; // Bending link - btScalar m_c0; // (ima+imb)*kLST - btScalar m_c1; // rl^2 - btScalar m_c2; // |gradient|^2/c0 - btVector3 m_c3; // gradient - }; - /* Face */ - struct Face : Feature - { - Node* m_n[3]; // Node pointers - btVector3 m_normal; // Normal - btScalar m_ra; // Rest area - btDbvtNode* m_leaf; // Leaf data - }; - /* RContact */ - struct RContact - { - sCti m_cti; // Contact infos - Node* m_node; // Owner node - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt - btScalar m_c3; // Friction - btScalar m_c4; // Hardness - }; - /* SContact */ - struct SContact - { - Node* m_node; // Node - Face* m_face; // Face - btVector3 m_weights; // Weigths - btVector3 m_normal; // Normal - btScalar m_margin; // Margin - btScalar m_friction; // Friction - btScalar m_cfm[2]; // Constraint force mixing - }; - /* Anchor */ - struct Anchor - { - Node* m_node; // Node pointer - btVector3 m_local; // Anchor position in body space - btRigidBody* m_body; // Body - btMatrix3x3 m_c0; // Impulse matrix - btVector3 m_c1; // Relative anchor - btScalar m_c2; // ima*dt - }; - /* Note */ - struct Note : Element - { - const char* m_text; // Text - btVector3 m_offset; // Offset - int m_rank; // Rank - Node* m_nodes[4]; // Nodes - btScalar m_coords[4]; // Coordinates - }; - /* Pose */ - struct Pose - { - bool m_bvolume; // Is valid - bool m_bframe; // Is frame - btScalar m_volume; // Rest volume - tVector3Array m_pos; // Reference positions - tScalarArray m_wgh; // Weights - btVector3 m_com; // COM - btMatrix3x3 m_rot; // Rotation - btMatrix3x3 m_scl; // Scale - btMatrix3x3 m_aqq; // Base scaling - }; - /* Cluster */ - struct Cluster - { - btAlignedObjectArray m_nodes; - tScalarArray m_masses; - tVector3Array m_framerefs; - btTransform m_framexform; - btScalar m_idmass; - btScalar m_imass; - btMatrix3x3 m_locii; - btMatrix3x3 m_invwi; - btVector3 m_com; - btVector3 m_vimpulses[2]; - btVector3 m_dimpulses[2]; - int m_nvimpulses; - int m_ndimpulses; - btVector3 m_lv; - btVector3 m_av; - btDbvtNode* m_leaf; - btScalar m_ndamping; /* Node damping */ - btScalar m_ldamping; /* Linear damping */ - btScalar m_adamping; /* Angular damping */ - btScalar m_matching; - bool m_collide; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} - }; - /* Impulse */ - struct Impulse - { - btVector3 m_velocity; - btVector3 m_drift; - int m_asVelocity:1; - int m_asDrift:1; - Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} - Impulse operator -() const - { - Impulse i=*this; - i.m_velocity=-i.m_velocity; - i.m_drift=-i.m_drift; - return(i); - } - Impulse operator*(btScalar x) const - { - Impulse i=*this; - i.m_velocity*=x; - i.m_drift*=x; - return(i); - } - }; - /* Body */ - struct Body - { - Cluster* m_soft; - btRigidBody* m_rigid; - btCollisionObject* m_collisionObject; - - Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} - Body(btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) - { - m_rigid = btRigidBody::upcast(m_collisionObject); - } - - void activate() const - { - if(m_rigid) m_rigid->activate(); - } - const btMatrix3x3& invWorldInertia() const - { - static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); - if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); - if(m_soft) return(m_soft->m_invwi); - return(iwi); - } - btScalar invMass() const - { - if(m_rigid) return(m_rigid->getInvMass()); - if(m_soft) return(m_soft->m_imass); - return(0); - } - const btTransform& xform() const - { - static const btTransform identity=btTransform::getIdentity(); - if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform()); - if(m_soft) return(m_soft->m_framexform); - return(identity); - } - btVector3 linearVelocity() const - { - if(m_rigid) return(m_rigid->getLinearVelocity()); - if(m_soft) return(m_soft->m_lv); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity(const btVector3& rpos) const - { - if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); - if(m_soft) return(cross(m_soft->m_av,rpos)); - return(btVector3(0,0,0)); - } - btVector3 angularVelocity() const - { - if(m_rigid) return(m_rigid->getAngularVelocity()); - if(m_soft) return(m_soft->m_av); - return(btVector3(0,0,0)); - } - btVector3 velocity(const btVector3& rpos) const - { - return(linearVelocity()+angularVelocity(rpos)); - } - void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } - void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { - if(m_rigid) m_rigid->applyImpulse(impulse,rpos); - if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } - void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { - if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); - if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); - } - void applyVAImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } - void applyDAImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyTorqueImpulse(impulse); - if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } - void applyAImpulse(const Impulse& impulse) const - { - if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); - if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } - void applyDCImpulse(const btVector3& impulse) const - { - if(m_rigid) m_rigid->applyCentralImpulse(impulse); - if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } - }; - /* Joint */ - struct Joint - { - struct eType { enum _ { - Linear, - Angular, - Contact - };}; - struct Specs - { - Specs() : erp(1),cfm(1),split(1) {} - btScalar erp; - btScalar cfm; - btScalar split; - }; - Body m_bodies[2]; - btVector3 m_refs[2]; - btScalar m_cfm; - btScalar m_erp; - btScalar m_split; - btVector3 m_drift; - btVector3 m_sdrift; - btMatrix3x3 m_massmatrix; - bool m_delete; - virtual ~Joint() {} - Joint() : m_delete(false) {} - virtual void Prepare(btScalar dt,int iterations); - virtual void Solve(btScalar dt,btScalar sor)=0; - virtual void Terminate(btScalar dt)=0; - virtual eType::_ Type() const=0; - }; - /* LJoint */ - struct LJoint : Joint - { - struct Specs : Joint::Specs - { - btVector3 position; - }; - btVector3 m_rpos[2]; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Linear); } - }; - /* AJoint */ - struct AJoint : Joint - { - struct IControl - { - virtual void Prepare(AJoint*) {} - virtual btScalar Speed(AJoint*,btScalar current) { return(current); } - static IControl* Default() { static IControl def;return(&def); } - }; - struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} - btVector3 axis; - IControl* icontrol; - }; - btVector3 m_axis[2]; - IControl* m_icontrol; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Angular); } - }; - /* CJoint */ - struct CJoint : Joint - { - int m_life; - int m_maxlife; - btVector3 m_rpos[2]; - btVector3 m_normal; - btScalar m_friction; - void Prepare(btScalar dt,int iterations); - void Solve(btScalar dt,btScalar sor); - void Terminate(btScalar dt); - eType::_ Type() const { return(eType::Contact); } - }; - /* Config */ - struct Config - { - eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) - btScalar kVCF; // Velocities correction factor (Baumgarte) - btScalar kDP; // Damping coefficient [0,1] - btScalar kDG; // Drag coefficient [0,+inf] - btScalar kLF; // Lift coefficient [0,+inf] - btScalar kPR; // Pressure coefficient [-inf,+inf] - btScalar kVC; // Volume conversation coefficient [0,+inf] - btScalar kDF; // Dynamic friction coefficient [0,1] - btScalar kMT; // Pose matching coefficient [0,1] - btScalar kCHR; // Rigid contacts hardness [0,1] - btScalar kKHR; // Kinetic contacts hardness [0,1] - btScalar kSHR; // Soft contacts hardness [0,1] - btScalar kAHR; // Anchors hardness [0,1] - btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) - btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) - btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) - btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) - btScalar maxvolume; // Maximum volume ratio for pose - btScalar timescale; // Time scale - int viterations; // Velocities solver iterations - int piterations; // Positions solver iterations - int diterations; // Drift solver iterations - int citerations; // Cluster solver iterations - int collisions; // Collisions flags - tVSolverArray m_vsequence; // Velocity solvers sequence - tPSolverArray m_psequence; // Position solvers sequence - tPSolverArray m_dsequence; // Drift solvers sequence - }; - /* SolverState */ - struct SolverState - { - btScalar sdt; // dt*timescale - btScalar isdt; // 1/sdt - btScalar velmrg; // velocity margin - btScalar radmrg; // radial margin - btScalar updmrg; // Update margin - }; - /// RayFromToCaster takes a ray from, ray to (instead of direction!) - struct RayFromToCaster : btDbvt::ICollide - { - btVector3 m_rayFrom; - btVector3 m_rayTo; - btVector3 m_rayNormalizedDirection; - btScalar m_mint; - Face* m_face; - int m_tests; - RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); - void Process(const btDbvtNode* leaf); - - static inline btScalar rayFromToTriangle(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); - }; - - // - // Typedef's - // - - typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); - typedef void (*vsolver_t)(btSoftBody*,btScalar); - typedef btAlignedObjectArray tClusterArray; - typedef btAlignedObjectArray tNoteArray; - typedef btAlignedObjectArray tNodeArray; - typedef btAlignedObjectArray tLeafArray; - typedef btAlignedObjectArray tLinkArray; - typedef btAlignedObjectArray tFaceArray; - typedef btAlignedObjectArray tAnchorArray; - typedef btAlignedObjectArray tRContactArray; - typedef btAlignedObjectArray tSContactArray; - typedef btAlignedObjectArray tMaterialArray; - typedef btAlignedObjectArray tJointArray; - typedef btAlignedObjectArray tSoftBodyArray; - - // - // Fields - // - - Config m_cfg; // Configuration - SolverState m_sst; // Solver state - Pose m_pose; // Pose - void* m_tag; // User data - btSoftBodyWorldInfo* m_worldInfo; // World info - tNoteArray m_notes; // Notes - tNodeArray m_nodes; // Nodes - tLinkArray m_links; // Links - tFaceArray m_faces; // Faces - tAnchorArray m_anchors; // Anchors - tRContactArray m_rcontacts; // Rigid contacts - tSContactArray m_scontacts; // Soft contacts - tJointArray m_joints; // Joints - tMaterialArray m_materials; // Materials - btScalar m_timeacc; // Time accumulator - btVector3 m_bounds[2]; // Spatial bounds - bool m_bUpdateRtCst; // Update runtime constants - btDbvt m_ndbvt; // Nodes tree - btDbvt m_fdbvt; // Faces tree - btDbvt m_cdbvt; // Clusters tree - tClusterArray m_clusters; // Clusters - - // - // Api - // - - /* ctor */ - btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, - const btVector3* x, - const btScalar* m); - /* dtor */ - virtual ~btSoftBody(); - /* Check for existing link */ - - btAlignedObjectArray m_userIndexMapping; - - btSoftBodyWorldInfo* getWorldInfo() - { - return m_worldInfo; - } - - ///@todo: avoid internal softbody shape hack and move collision code to collision library - virtual void setCollisionShape(btCollisionShape* collisionShape) - { - - } - - bool checkLink( int node0, - int node1) const; - bool checkLink( const Node* node0, - const Node* node1) const; - /* Check for existring face */ - bool checkFace( int node0, - int node1, - int node2) const; - /* Append material */ - Material* appendMaterial(); - /* Append note */ - void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); - void appendNote( const char* text, - const btVector3& o, - Node* feature); - void appendNote( const char* text, - const btVector3& o, - Link* feature); - void appendNote( const char* text, - const btVector3& o, - Face* feature); - /* Append node */ - void appendNode( const btVector3& x,btScalar m); - /* Append link */ - void appendLink(int model=-1,Material* mat=0); - void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); - void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); - /* Append face */ - void appendFace(int model=-1,Material* mat=0); - void appendFace( int node0, - int node1, - int node2, - Material* mat=0); - /* Append anchor */ - void appendAnchor( int node, - btRigidBody* body); - /* Append linear joint */ - void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); - void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); - void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body); - /* Append linear joint */ - void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1); - void appendAngularJoint(const AJoint::Specs& specs,Body body=Body()); - void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body); - /* Add force (or gravity) to the entire body */ - void addForce( const btVector3& force); - /* Add force (or gravity) to a node of the body */ - void addForce( const btVector3& force, - int node); - /* Add velocity to the entire body */ - void addVelocity( const btVector3& velocity); - - /* Set velocity for the entire body */ - void setVelocity( const btVector3& velocity); - - /* Add velocity to a node of the body */ - void addVelocity( const btVector3& velocity, - int node); - /* Set mass */ - void setMass( int node, - btScalar mass); - /* Get mass */ - btScalar getMass( int node) const; - /* Get total mass */ - btScalar getTotalMass() const; - /* Set total mass (weighted by previous masses) */ - void setTotalMass( btScalar mass, - bool fromfaces=false); - /* Set total density */ - void setTotalDensity(btScalar density); - /* Transform */ - void transform( const btTransform& trs); - /* Translate */ - void translate( const btVector3& trs); - /* Rotate */ - void rotate( const btQuaternion& rot); - /* Scale */ - void scale( const btVector3& scl); - /* Set current state as pose */ - void setPose( bool bvolume, - bool bframe); - /* Return the volume */ - btScalar getVolume() const; - /* Cluster count */ - int clusterCount() const; - /* Cluster center of mass */ - static btVector3 clusterCom(const Cluster* cluster); - btVector3 clusterCom(int cluster) const; - /* Cluster velocity at rpos */ - static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos); - /* Cluster impulse */ - static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); - static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse); - static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse); - static void clusterAImpulse(Cluster* cluster,const Impulse& impulse); - static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); - /* Generate bending constraints based on distance in the adjency graph */ - int generateBendingConstraints( int distance, - Material* mat=0); - /* Randomize constraints to reduce solver bias */ - void randomizeConstraints(); - /* Release clusters */ - void releaseCluster(int index); - void releaseClusters(); - /* Generate clusters (K-mean) */ - int generateClusters(int k,int maxiterations=8192); - /* Refine */ - void refine(ImplicitFn* ifn,btScalar accurary,bool cut); - /* CutLink */ - bool cutLink(int node0,int node1,btScalar position); - bool cutLink(const Node* node0,const Node* node1,btScalar position); - - ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) - bool rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results); - /* Solver presets */ - void setSolver(eSolverPresets::_ preset); - /* predictMotion */ - void predictMotion(btScalar dt); - /* solveConstraints */ - void solveConstraints(); - /* staticSolve */ - void staticSolve(int iterations); - /* solveCommonConstraints */ - static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); - /* solveClusters */ - static void solveClusters(const btAlignedObjectArray& bodies); - /* integrateMotion */ - void integrateMotion(); - /* defaultCollisionHandlers */ - void defaultCollisionHandler(btCollisionObject* pco); - void defaultCollisionHandler(btSoftBody* psb); - - // - // Cast - // - - static const btSoftBody* upcast(const btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (const btSoftBody*)colObj; - return 0; - } - static btSoftBody* upcast(btCollisionObject* colObj) - { - if (colObj->getInternalType()==CO_SOFT_BODY) - return (btSoftBody*)colObj; - return 0; - } - - // - // ::btCollisionObject - // - - virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const - { - aabbMin = m_bounds[0]; - aabbMax = m_bounds[1]; - } - // - // Private - // - void pointersToIndices(); - void indicesToPointers(const int* map=0); - - int rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; - void initializeFaceTree(); - btVector3 evaluateCom() const; - bool checkContact(btCollisionObject* colObj,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; - void updateNormals(); - void updateBounds(); - void updatePose(); - void updateConstants(); - void initializeClusters(); - void updateClusters(); - void cleanupClusters(); - void prepareClusters(int iterations); - void solveClusters(btScalar sor); - void applyClusters(bool drift); - void dampClusters(); - void applyForces(); - static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti); - static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti); - static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti); - static void VSolve_Links(btSoftBody* psb,btScalar kst); - static psolver_t getSolver(ePSolver::_ solver); - static vsolver_t getSolver(eVSolver::_ solver); - -}; - - - -#endif //_BT_SOFT_BODY_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +#ifndef _BT_SOFT_BODY_H +#define _BT_SOFT_BODY_H + +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btIDebugDraw.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "btSparseSDF.h" +#include "BulletCollision/BroadphaseCollision/btDbvt.h" + +class btBroadphaseInterface; +class btDispatcher; + +/* btSoftBodyWorldInfo */ +struct btSoftBodyWorldInfo +{ + btScalar air_density; + btScalar water_density; + btScalar water_offset; + btVector3 water_normal; + btBroadphaseInterface* m_broadphase; + btDispatcher* m_dispatcher; + btVector3 m_gravity; + btSparseSdf<3> m_sparsesdf; +}; + + +///The btSoftBody is an class to simulate cloth and volumetric soft bodies. +///There is two-way interaction between btSoftBody and btRigidBody/btCollisionObject. +class btSoftBody : public btCollisionObject +{ +public: + // + // Enumerations + // + + ///eAeroModel + struct eAeroModel { enum _ { + V_Point, ///Vertex normals are oriented toward velocity + V_TwoSided, ///Vertex normals are fliped to match velocity + V_OneSided, ///Vertex normals are taken as it is + F_TwoSided, ///Face normals are fliped to match velocity + F_OneSided, ///Face normals are taken as it is + END + };}; + + ///eVSolver : velocities solvers + struct eVSolver { enum _ { + Linear, ///Linear solver + END + };}; + + ///ePSolver : positions solvers + struct ePSolver { enum _ { + Linear, ///Linear solver + Anchors, ///Anchor solver + RContacts, ///Rigid contacts solver + SContacts, ///Soft contacts solver + END + };}; + + ///eSolverPresets + struct eSolverPresets { enum _ { + Positions, + Velocities, + Default = Positions, + END + };}; + + ///eFeature + struct eFeature { enum _ { + None, + Node, + Link, + Face, + END + };}; + + typedef btAlignedObjectArray tVSolverArray; + typedef btAlignedObjectArray tPSolverArray; + + // + // Flags + // + + ///fCollision + struct fCollision { enum _ { + RVSmask = 0x000f, ///Rigid versus soft mask + SDF_RS = 0x0001, ///SDF based rigid vs soft + CL_RS = 0x0002, ///Cluster vs convex rigid vs soft + + SVSmask = 0x00f0, ///Rigid versus soft mask + VF_SS = 0x0010, ///Vertex vs face soft vs soft handling + CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling + /* presets */ + Default = SDF_RS, + END + };}; + + ///fMaterial + struct fMaterial { enum _ { + DebugDraw = 0x0001, /// Enable debug draw + /* presets */ + Default = DebugDraw, + END + };}; + + // + // API Types + // + + /* sRayCast */ + struct sRayCast + { + btSoftBody* body; /// soft body + eFeature::_ feature; /// feature type + int index; /// feature index + btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) + }; + + /* ImplicitFn */ + struct ImplicitFn + { + virtual btScalar Eval(const btVector3& x)=0; + }; + + // + // Internal types + // + + typedef btAlignedObjectArray tScalarArray; + typedef btAlignedObjectArray tVector3Array; + + /* sCti is Softbody contact info */ + struct sCti + { + btCollisionObject* m_colObj; /* Rigid body */ + btVector3 m_normal; /* Outward normal */ + btScalar m_offset; /* Offset from origin */ + }; + + /* sMedium */ + struct sMedium + { + btVector3 m_velocity; /* Velocity */ + btScalar m_pressure; /* Pressure */ + btScalar m_density; /* Density */ + }; + + /* Base type */ + struct Element + { + void* m_tag; // User data + Element() : m_tag(0) {} + }; + /* Material */ + struct Material : Element + { + btScalar m_kLST; // Linear stiffness coefficient [0,1] + btScalar m_kAST; // Area/Angular stiffness coefficient [0,1] + btScalar m_kVST; // Volume stiffness coefficient [0,1] + int m_flags; // Flags + }; + + /* Feature */ + struct Feature : Element + { + Material* m_material; // Material + }; + /* Node */ + struct Node : Feature + { + btVector3 m_x; // Position + btVector3 m_q; // Previous step position + btVector3 m_v; // Velocity + btVector3 m_f; // Force accumulator + btVector3 m_n; // Normal + btScalar m_im; // 1/mass + btScalar m_area; // Area + btDbvtNode* m_leaf; // Leaf data + int m_battach:1; // Attached + }; + /* Link */ + struct Link : Feature + { + Node* m_n[2]; // Node pointers + btScalar m_rl; // Rest length + int m_bbending:1; // Bending link + btScalar m_c0; // (ima+imb)*kLST + btScalar m_c1; // rl^2 + btScalar m_c2; // |gradient|^2/c0 + btVector3 m_c3; // gradient + }; + /* Face */ + struct Face : Feature + { + Node* m_n[3]; // Node pointers + btVector3 m_normal; // Normal + btScalar m_ra; // Rest area + btDbvtNode* m_leaf; // Leaf data + }; + /* RContact */ + struct RContact + { + sCti m_cti; // Contact infos + Node* m_node; // Owner node + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt + btScalar m_c3; // Friction + btScalar m_c4; // Hardness + }; + /* SContact */ + struct SContact + { + Node* m_node; // Node + Face* m_face; // Face + btVector3 m_weights; // Weigths + btVector3 m_normal; // Normal + btScalar m_margin; // Margin + btScalar m_friction; // Friction + btScalar m_cfm[2]; // Constraint force mixing + }; + /* Anchor */ + struct Anchor + { + Node* m_node; // Node pointer + btVector3 m_local; // Anchor position in body space + btRigidBody* m_body; // Body + btMatrix3x3 m_c0; // Impulse matrix + btVector3 m_c1; // Relative anchor + btScalar m_c2; // ima*dt + }; + /* Note */ + struct Note : Element + { + const char* m_text; // Text + btVector3 m_offset; // Offset + int m_rank; // Rank + Node* m_nodes[4]; // Nodes + btScalar m_coords[4]; // Coordinates + }; + /* Pose */ + struct Pose + { + bool m_bvolume; // Is valid + bool m_bframe; // Is frame + btScalar m_volume; // Rest volume + tVector3Array m_pos; // Reference positions + tScalarArray m_wgh; // Weights + btVector3 m_com; // COM + btMatrix3x3 m_rot; // Rotation + btMatrix3x3 m_scl; // Scale + btMatrix3x3 m_aqq; // Base scaling + }; + /* Cluster */ + struct Cluster + { + btAlignedObjectArray m_nodes; + tScalarArray m_masses; + tVector3Array m_framerefs; + btTransform m_framexform; + btScalar m_idmass; + btScalar m_imass; + btMatrix3x3 m_locii; + btMatrix3x3 m_invwi; + btVector3 m_com; + btVector3 m_vimpulses[2]; + btVector3 m_dimpulses[2]; + int m_nvimpulses; + int m_ndimpulses; + btVector3 m_lv; + btVector3 m_av; + btDbvtNode* m_leaf; + btScalar m_ndamping; /* Node damping */ + btScalar m_ldamping; /* Linear damping */ + btScalar m_adamping; /* Angular damping */ + btScalar m_matching; + bool m_collide; + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + }; + /* Impulse */ + struct Impulse + { + btVector3 m_velocity; + btVector3 m_drift; + int m_asVelocity:1; + int m_asDrift:1; + Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} + Impulse operator -() const + { + Impulse i=*this; + i.m_velocity=-i.m_velocity; + i.m_drift=-i.m_drift; + return(i); + } + Impulse operator*(btScalar x) const + { + Impulse i=*this; + i.m_velocity*=x; + i.m_drift*=x; + return(i); + } + }; + /* Body */ + struct Body + { + Cluster* m_soft; + btRigidBody* m_rigid; + btCollisionObject* m_collisionObject; + + Body() : m_soft(0),m_rigid(0),m_collisionObject(0) {} + Body(Cluster* p) : m_soft(p),m_rigid(0),m_collisionObject(0) {} + Body(btCollisionObject* colObj) : m_soft(0),m_collisionObject(colObj) + { + m_rigid = btRigidBody::upcast(m_collisionObject); + } + + void activate() const + { + if(m_rigid) m_rigid->activate(); + } + const btMatrix3x3& invWorldInertia() const + { + static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); + if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); + if(m_soft) return(m_soft->m_invwi); + return(iwi); + } + btScalar invMass() const + { + if(m_rigid) return(m_rigid->getInvMass()); + if(m_soft) return(m_soft->m_imass); + return(0); + } + const btTransform& xform() const + { + static const btTransform identity=btTransform::getIdentity(); + if(m_collisionObject) return(m_collisionObject->getInterpolationWorldTransform()); + if(m_soft) return(m_soft->m_framexform); + return(identity); + } + btVector3 linearVelocity() const + { + if(m_rigid) return(m_rigid->getLinearVelocity()); + if(m_soft) return(m_soft->m_lv); + return(btVector3(0,0,0)); + } + btVector3 angularVelocity(const btVector3& rpos) const + { + if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); + if(m_soft) return(cross(m_soft->m_av,rpos)); + return(btVector3(0,0,0)); + } + btVector3 angularVelocity() const + { + if(m_rigid) return(m_rigid->getAngularVelocity()); + if(m_soft) return(m_soft->m_av); + return(btVector3(0,0,0)); + } + btVector3 velocity(const btVector3& rpos) const + { + return(linearVelocity()+angularVelocity(rpos)); + } + void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const + { + if(m_rigid) m_rigid->applyImpulse(impulse,rpos); + if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); + } + void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const + { + if(m_rigid) m_rigid->applyImpulse(impulse,rpos); + if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); + } + void applyImpulse(const Impulse& impulse,const btVector3& rpos) const + { + if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); + if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); + } + void applyVAImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyTorqueImpulse(impulse); + if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); + } + void applyDAImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyTorqueImpulse(impulse); + if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); + } + void applyAImpulse(const Impulse& impulse) const + { + if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); + if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); + } + void applyDCImpulse(const btVector3& impulse) const + { + if(m_rigid) m_rigid->applyCentralImpulse(impulse); + if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); + } + }; + /* Joint */ + struct Joint + { + struct eType { enum _ { + Linear, + Angular, + Contact + };}; + struct Specs + { + Specs() : erp(1),cfm(1),split(1) {} + btScalar erp; + btScalar cfm; + btScalar split; + }; + Body m_bodies[2]; + btVector3 m_refs[2]; + btScalar m_cfm; + btScalar m_erp; + btScalar m_split; + btVector3 m_drift; + btVector3 m_sdrift; + btMatrix3x3 m_massmatrix; + bool m_delete; + virtual ~Joint() {} + Joint() : m_delete(false) {} + virtual void Prepare(btScalar dt,int iterations); + virtual void Solve(btScalar dt,btScalar sor)=0; + virtual void Terminate(btScalar dt)=0; + virtual eType::_ Type() const=0; + }; + /* LJoint */ + struct LJoint : Joint + { + struct Specs : Joint::Specs + { + btVector3 position; + }; + btVector3 m_rpos[2]; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Linear); } + }; + /* AJoint */ + struct AJoint : Joint + { + struct IControl + { + virtual void Prepare(AJoint*) {} + virtual btScalar Speed(AJoint*,btScalar current) { return(current); } + static IControl* Default() { static IControl def;return(&def); } + }; + struct Specs : Joint::Specs + { + Specs() : icontrol(IControl::Default()) {} + btVector3 axis; + IControl* icontrol; + }; + btVector3 m_axis[2]; + IControl* m_icontrol; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Angular); } + }; + /* CJoint */ + struct CJoint : Joint + { + int m_life; + int m_maxlife; + btVector3 m_rpos[2]; + btVector3 m_normal; + btScalar m_friction; + void Prepare(btScalar dt,int iterations); + void Solve(btScalar dt,btScalar sor); + void Terminate(btScalar dt); + eType::_ Type() const { return(eType::Contact); } + }; + /* Config */ + struct Config + { + eAeroModel::_ aeromodel; // Aerodynamic model (default: V_Point) + btScalar kVCF; // Velocities correction factor (Baumgarte) + btScalar kDP; // Damping coefficient [0,1] + btScalar kDG; // Drag coefficient [0,+inf] + btScalar kLF; // Lift coefficient [0,+inf] + btScalar kPR; // Pressure coefficient [-inf,+inf] + btScalar kVC; // Volume conversation coefficient [0,+inf] + btScalar kDF; // Dynamic friction coefficient [0,1] + btScalar kMT; // Pose matching coefficient [0,1] + btScalar kCHR; // Rigid contacts hardness [0,1] + btScalar kKHR; // Kinetic contacts hardness [0,1] + btScalar kSHR; // Soft contacts hardness [0,1] + btScalar kAHR; // Anchors hardness [0,1] + btScalar kSRHR_CL; // Soft vs rigid hardness [0,1] (cluster only) + btScalar kSKHR_CL; // Soft vs kinetic hardness [0,1] (cluster only) + btScalar kSSHR_CL; // Soft vs soft hardness [0,1] (cluster only) + btScalar kSR_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSK_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar kSS_SPLT_CL; // Soft vs rigid impulse split [0,1] (cluster only) + btScalar maxvolume; // Maximum volume ratio for pose + btScalar timescale; // Time scale + int viterations; // Velocities solver iterations + int piterations; // Positions solver iterations + int diterations; // Drift solver iterations + int citerations; // Cluster solver iterations + int collisions; // Collisions flags + tVSolverArray m_vsequence; // Velocity solvers sequence + tPSolverArray m_psequence; // Position solvers sequence + tPSolverArray m_dsequence; // Drift solvers sequence + }; + /* SolverState */ + struct SolverState + { + btScalar sdt; // dt*timescale + btScalar isdt; // 1/sdt + btScalar velmrg; // velocity margin + btScalar radmrg; // radial margin + btScalar updmrg; // Update margin + }; + /// RayFromToCaster takes a ray from, ray to (instead of direction!) + struct RayFromToCaster : btDbvt::ICollide + { + btVector3 m_rayFrom; + btVector3 m_rayTo; + btVector3 m_rayNormalizedDirection; + btScalar m_mint; + Face* m_face; + int m_tests; + RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); + void Process(const btDbvtNode* leaf); + + static inline btScalar rayFromToTriangle(const btVector3& rayFrom, + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY); + }; + + // + // Typedef's + // + + typedef void (*psolver_t)(btSoftBody*,btScalar,btScalar); + typedef void (*vsolver_t)(btSoftBody*,btScalar); + typedef btAlignedObjectArray tClusterArray; + typedef btAlignedObjectArray tNoteArray; + typedef btAlignedObjectArray tNodeArray; + typedef btAlignedObjectArray tLeafArray; + typedef btAlignedObjectArray tLinkArray; + typedef btAlignedObjectArray tFaceArray; + typedef btAlignedObjectArray tAnchorArray; + typedef btAlignedObjectArray tRContactArray; + typedef btAlignedObjectArray tSContactArray; + typedef btAlignedObjectArray tMaterialArray; + typedef btAlignedObjectArray tJointArray; + typedef btAlignedObjectArray tSoftBodyArray; + + // + // Fields + // + + Config m_cfg; // Configuration + SolverState m_sst; // Solver state + Pose m_pose; // Pose + void* m_tag; // User data + btSoftBodyWorldInfo* m_worldInfo; // World info + tNoteArray m_notes; // Notes + tNodeArray m_nodes; // Nodes + tLinkArray m_links; // Links + tFaceArray m_faces; // Faces + tAnchorArray m_anchors; // Anchors + tRContactArray m_rcontacts; // Rigid contacts + tSContactArray m_scontacts; // Soft contacts + tJointArray m_joints; // Joints + tMaterialArray m_materials; // Materials + btScalar m_timeacc; // Time accumulator + btVector3 m_bounds[2]; // Spatial bounds + bool m_bUpdateRtCst; // Update runtime constants + btDbvt m_ndbvt; // Nodes tree + btDbvt m_fdbvt; // Faces tree + btDbvt m_cdbvt; // Clusters tree + tClusterArray m_clusters; // Clusters + + // + // Api + // + + /* ctor */ + btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, + const btVector3* x, + const btScalar* m); + /* dtor */ + virtual ~btSoftBody(); + /* Check for existing link */ + + btAlignedObjectArray m_userIndexMapping; + + btSoftBodyWorldInfo* getWorldInfo() + { + return m_worldInfo; + } + + ///@todo: avoid internal softbody shape hack and move collision code to collision library + virtual void setCollisionShape(btCollisionShape* collisionShape) + { + + } + + bool checkLink( int node0, + int node1) const; + bool checkLink( const Node* node0, + const Node* node1) const; + /* Check for existring face */ + bool checkFace( int node0, + int node1, + int node2) const; + /* Append material */ + Material* appendMaterial(); + /* Append note */ + void appendNote( const char* text, + const btVector3& o, + const btVector4& c=btVector4(1,0,0,0), + Node* n0=0, + Node* n1=0, + Node* n2=0, + Node* n3=0); + void appendNote( const char* text, + const btVector3& o, + Node* feature); + void appendNote( const char* text, + const btVector3& o, + Link* feature); + void appendNote( const char* text, + const btVector3& o, + Face* feature); + /* Append node */ + void appendNode( const btVector3& x,btScalar m); + /* Append link */ + void appendLink(int model=-1,Material* mat=0); + void appendLink( int node0, + int node1, + Material* mat=0, + bool bcheckexist=false); + void appendLink( Node* node0, + Node* node1, + Material* mat=0, + bool bcheckexist=false); + /* Append face */ + void appendFace(int model=-1,Material* mat=0); + void appendFace( int node0, + int node1, + int node2, + Material* mat=0); + /* Append anchor */ + void appendAnchor( int node, + btRigidBody* body); + /* Append linear joint */ + void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); + void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); + void appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body); + /* Append linear joint */ + void appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1); + void appendAngularJoint(const AJoint::Specs& specs,Body body=Body()); + void appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body); + /* Add force (or gravity) to the entire body */ + void addForce( const btVector3& force); + /* Add force (or gravity) to a node of the body */ + void addForce( const btVector3& force, + int node); + /* Add velocity to the entire body */ + void addVelocity( const btVector3& velocity); + + /* Set velocity for the entire body */ + void setVelocity( const btVector3& velocity); + + /* Add velocity to a node of the body */ + void addVelocity( const btVector3& velocity, + int node); + /* Set mass */ + void setMass( int node, + btScalar mass); + /* Get mass */ + btScalar getMass( int node) const; + /* Get total mass */ + btScalar getTotalMass() const; + /* Set total mass (weighted by previous masses) */ + void setTotalMass( btScalar mass, + bool fromfaces=false); + /* Set total density */ + void setTotalDensity(btScalar density); + /* Transform */ + void transform( const btTransform& trs); + /* Translate */ + void translate( const btVector3& trs); + /* Rotate */ + void rotate( const btQuaternion& rot); + /* Scale */ + void scale( const btVector3& scl); + /* Set current state as pose */ + void setPose( bool bvolume, + bool bframe); + /* Return the volume */ + btScalar getVolume() const; + /* Cluster count */ + int clusterCount() const; + /* Cluster center of mass */ + static btVector3 clusterCom(const Cluster* cluster); + btVector3 clusterCom(int cluster) const; + /* Cluster velocity at rpos */ + static btVector3 clusterVelocity(const Cluster* cluster,const btVector3& rpos); + /* Cluster impulse */ + static void clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); + static void clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse); + static void clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse); + static void clusterVAImpulse(Cluster* cluster,const btVector3& impulse); + static void clusterDAImpulse(Cluster* cluster,const btVector3& impulse); + static void clusterAImpulse(Cluster* cluster,const Impulse& impulse); + static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); + /* Generate bending constraints based on distance in the adjency graph */ + int generateBendingConstraints( int distance, + Material* mat=0); + /* Randomize constraints to reduce solver bias */ + void randomizeConstraints(); + /* Release clusters */ + void releaseCluster(int index); + void releaseClusters(); + /* Generate clusters (K-mean) */ + int generateClusters(int k,int maxiterations=8192); + /* Refine */ + void refine(ImplicitFn* ifn,btScalar accurary,bool cut); + /* CutLink */ + bool cutLink(int node0,int node1,btScalar position); + bool cutLink(const Node* node0,const Node* node1,btScalar position); + + ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) + bool rayTest(const btVector3& rayFrom, + const btVector3& rayTo, + sRayCast& results); + /* Solver presets */ + void setSolver(eSolverPresets::_ preset); + /* predictMotion */ + void predictMotion(btScalar dt); + /* solveConstraints */ + void solveConstraints(); + /* staticSolve */ + void staticSolve(int iterations); + /* solveCommonConstraints */ + static void solveCommonConstraints(btSoftBody** bodies,int count,int iterations); + /* solveClusters */ + static void solveClusters(const btAlignedObjectArray& bodies); + /* integrateMotion */ + void integrateMotion(); + /* defaultCollisionHandlers */ + void defaultCollisionHandler(btCollisionObject* pco); + void defaultCollisionHandler(btSoftBody* psb); + + // + // Cast + // + + static const btSoftBody* upcast(const btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_SOFT_BODY) + return (const btSoftBody*)colObj; + return 0; + } + static btSoftBody* upcast(btCollisionObject* colObj) + { + if (colObj->getInternalType()==CO_SOFT_BODY) + return (btSoftBody*)colObj; + return 0; + } + + // + // ::btCollisionObject + // + + virtual void getAabb(btVector3& aabbMin,btVector3& aabbMax) const + { + aabbMin = m_bounds[0]; + aabbMax = m_bounds[1]; + } + // + // Private + // + void pointersToIndices(); + void indicesToPointers(const int* map=0); + + int rayTest(const btVector3& rayFrom,const btVector3& rayTo, + btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; + void initializeFaceTree(); + btVector3 evaluateCom() const; + bool checkContact(btCollisionObject* colObj,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; + void updateNormals(); + void updateBounds(); + void updatePose(); + void updateConstants(); + void initializeClusters(); + void updateClusters(); + void cleanupClusters(); + void prepareClusters(int iterations); + void solveClusters(btScalar sor); + void applyClusters(bool drift); + void dampClusters(); + void applyForces(); + static void PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti); + static void PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti); + static void PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti); + static void PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti); + static void VSolve_Links(btSoftBody* psb,btScalar kst); + static psolver_t getSolver(ePSolver::_ solver); + static vsolver_t getSolver(eVSolver::_ solver); + +}; + + + +#endif //_BT_SOFT_BODY_H diff --git a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 20b7b9c05..f334e15e0 100644 --- a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -1,369 +1,369 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" - - - -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletSoftBody/btSoftBody.h" - -#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable - -btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) -: btCollisionAlgorithm(ci), -m_isSwapped(isSwapped), -m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) -{ -} - - - -btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() -{ -} - - - -btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): -m_dispatcher(dispatcher), -m_dispatchInfoPtr(0) -{ - m_softBody = (btSoftBody*) (isSwapped? body1:body0); - m_triBody = isSwapped? body0:body1; - - // - // create the manifold from the dispatcher 'manifold pool' - // - // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - - clearCache(); -} - -btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() -{ - clearCache(); - // m_dispatcher->releaseManifold( m_manifoldPtr ); - -} - - -void btSoftBodyTriangleCallback::clearCache() -{ - for (int i=0;im_childShape); - m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? - delete tmp->m_childShape; - } - m_shapeCache.clear(); -} - - -void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) -{ - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - btCollisionObject* ob = static_cast(m_triBody); - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher1 = m_dispatcher; - - ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) - { - btVector3 color(255,255,0); - btTransform& tr = ob->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - } - - btTriIndex triIndex(partId,triangleIndex,0); - btHashKey triKey(triIndex.getUid()); - - - btTriIndex* shapeIndex = m_shapeCache[triKey]; - if (shapeIndex) - { - btCollisionShape* tm = shapeIndex->m_childShape; - btAssert(tm); - - //copy over user pointers to temporary shape - tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - - btCollisionShape* tmpShape = ob->getCollisionShape(); - ob->internalSetTemporaryCollisionShape( tm ); - - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); - - colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - ob->internalSetTemporaryCollisionShape( tmpShape); - return; - } - - //aabb filter is already applied! - - //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); - - // if (m_softBody->getCollisionShape()->getShapeType()== - { - // btVector3 other; - btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); - normal.normalize(); - normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; - // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; - // other+=normal*22.f; - btVector3 pts[6] = {triangle[0]+normal, - triangle[1]+normal, - triangle[2]+normal, - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; - - btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); - - - // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - - //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); - // tm.setMargin(m_collisionMarginTriangle); - - //copy over user pointers to temporary shape - tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - - btCollisionShape* tmpShape = ob->getCollisionShape(); - ob->internalSetTemporaryCollisionShape( tm ); - - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); - ///this should use the btDispatcher, so the actual registered algorithm is used - // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); - - //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - - ob->internalSetTemporaryCollisionShape( tmpShape ); - triIndex.m_childShape = tm; - m_shapeCache.insert(triKey,triIndex); - - } - - - -} - - - -void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); - m_resultOut = resultOut; - - - btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; - m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); - btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); - btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); - - btTransform softTransform; - softTransform.setIdentity(); - softTransform.setOrigin(softBodyCenter); - - btTransform convexInTriangleSpace; - convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform; - btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); -} - -void btSoftBodyConcaveCollisionAlgorithm::clearCache() -{ - m_btSoftBodyTriangleCallback.clearCache(); - -} - -void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - - - btCollisionObject* convexBody = m_isSwapped ? body1 : body0; - btCollisionObject* triBody = m_isSwapped ? body0 : body1; - - if (triBody->getCollisionShape()->isConcave()) - { - - - btCollisionObject* triOb = triBody; - btConcaveShape* concaveShape = static_cast( triOb->getCollisionShape()); - - // if (convexBody->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); - - //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. - //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); - - // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); - - - concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - - } - - } - -} - - -btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - btCollisionObject* convexbody = m_isSwapped ? body1 : body0; - btCollisionObject* triBody = m_isSwapped ? body0 : body1; - - - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - - //only perform CCD above a certain threshold, this prevents blocking on the long run - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) - { - return btScalar(1.); - } - - //const btVector3& from = convexbody->m_worldTransform.getOrigin(); - //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); - //todo: only do if the motion exceeds the 'radius' - - btTransform triInv = triBody->getWorldTransform().inverse(); - btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); - btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - - struct LocalTriangleSphereCastCallback : public btTriangleCallback - { - btTransform m_ccdSphereFromTrans; - btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; - - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - - - LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) - :m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { - } - - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - //do a swept sphere for now - btTransform ident; - ident.setIdentity(); - btConvexCast::CastResult castResult; - castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, - ident,ident,castResult)) - { - if (m_hitFraction > castResult.m_fraction) - m_hitFraction = castResult.m_fraction; - } - - } - - }; - - - - - - if (triBody->getCollisionShape()->isConcave()) - { - btVector3 rayAabbMin = convexFromLocal.getOrigin(); - rayAabbMin.setMin(convexToLocal.getOrigin()); - btVector3 rayAabbMax = convexFromLocal.getOrigin(); - rayAabbMax.setMax(convexToLocal.getOrigin()); - btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); - - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, - convexbody->getCcdSweptSphereRadius(),curHitFraction); - - raycastCallback.m_hitFraction = convexbody->getHitFraction(); - - btCollisionObject* concavebody = triBody; - - btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - - if (triangleMesh) - { - triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); - } - - - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) - { - convexbody->setHitFraction( raycastCallback.m_hitFraction); - return raycastCallback.m_hitFraction; - } - } - - return btScalar(1.); - -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSoftBodyConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" + + + +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletSoftBody/btSoftBody.h" + +#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable + +btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) +: btCollisionAlgorithm(ci), +m_isSwapped(isSwapped), +m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) +{ +} + + + +btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() +{ +} + + + +btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): +m_dispatcher(dispatcher), +m_dispatchInfoPtr(0) +{ + m_softBody = (btSoftBody*) (isSwapped? body1:body0); + m_triBody = isSwapped? body0:body1; + + // + // create the manifold from the dispatcher 'manifold pool' + // + // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); + + clearCache(); +} + +btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() +{ + clearCache(); + // m_dispatcher->releaseManifold( m_manifoldPtr ); + +} + + +void btSoftBodyTriangleCallback::clearCache() +{ + for (int i=0;im_childShape); + m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? + delete tmp->m_childShape; + } + m_shapeCache.clear(); +} + + +void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +{ + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); + btCollisionObject* ob = static_cast(m_triBody); + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher1 = m_dispatcher; + + ///debug drawing of the overlapping triangles + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) + { + btVector3 color(255,255,0); + btTransform& tr = ob->getWorldTransform(); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); + m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); + } + + btTriIndex triIndex(partId,triangleIndex,0); + btHashKey triKey(triIndex.getUid()); + + + btTriIndex* shapeIndex = m_shapeCache[triKey]; + if (shapeIndex) + { + btCollisionShape* tm = shapeIndex->m_childShape; + btAssert(tm); + + //copy over user pointers to temporary shape + tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); + + btCollisionShape* tmpShape = ob->getCollisionShape(); + ob->internalSetTemporaryCollisionShape( tm ); + + + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); + + colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->~btCollisionAlgorithm(); + ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); + ob->internalSetTemporaryCollisionShape( tmpShape); + return; + } + + //aabb filter is already applied! + + //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); + + // if (m_softBody->getCollisionShape()->getShapeType()== + { + // btVector3 other; + btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); + normal.normalize(); + normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; + // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; + // other+=normal*22.f; + btVector3 pts[6] = {triangle[0]+normal, + triangle[1]+normal, + triangle[2]+normal, + triangle[0]-normal, + triangle[1]-normal, + triangle[2]-normal}; + + btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); + + + // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); + + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + // tm.setMargin(m_collisionMarginTriangle); + + //copy over user pointers to temporary shape + tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); + + btCollisionShape* tmpShape = ob->getCollisionShape(); + ob->internalSetTemporaryCollisionShape( tm ); + + + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); + ///this should use the btDispatcher, so the actual registered algorithm is used + // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); + + //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo->~btCollisionAlgorithm(); + ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); + + + ob->internalSetTemporaryCollisionShape( tmpShape ); + triIndex.m_childShape = tm; + m_shapeCache.insert(triKey,triIndex); + + } + + + +} + + + +void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + m_dispatchInfoPtr = &dispatchInfo; + m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); + m_resultOut = resultOut; + + + btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; + m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); + btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); + btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); + + btTransform softTransform; + softTransform.setIdentity(); + softTransform.setOrigin(softBodyCenter); + + btTransform convexInTriangleSpace; + convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform; + btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); +} + +void btSoftBodyConcaveCollisionAlgorithm::clearCache() +{ + m_btSoftBodyTriangleCallback.clearCache(); + +} + +void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + + btCollisionObject* convexBody = m_isSwapped ? body1 : body0; + btCollisionObject* triBody = m_isSwapped ? body0 : body1; + + if (triBody->getCollisionShape()->isConcave()) + { + + + btCollisionObject* triOb = triBody; + btConcaveShape* concaveShape = static_cast( triOb->getCollisionShape()); + + // if (convexBody->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); + + // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); + m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); + + //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. + //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); + + // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); + + + concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); + + // resultOut->refreshContactPoints(); + + } + + } + +} + + +btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + btCollisionObject* convexbody = m_isSwapped ? body1 : body0; + btCollisionObject* triBody = m_isSwapped ? body0 : body1; + + + //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) + + //only perform CCD above a certain threshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); + if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) + { + return btScalar(1.); + } + + //const btVector3& from = convexbody->m_worldTransform.getOrigin(); + //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //todo: only do if the motion exceeds the 'radius' + + btTransform triInv = triBody->getWorldTransform().inverse(); + btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); + btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); + + struct LocalTriangleSphereCastCallback : public btTriangleCallback + { + btTransform m_ccdSphereFromTrans; + btTransform m_ccdSphereToTrans; + btTransform m_meshTransform; + + btScalar m_ccdSphereRadius; + btScalar m_hitFraction; + + + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) + :m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { + } + + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + //do a swept sphere for now + btTransform ident; + ident.setIdentity(); + btConvexCast::CastResult castResult; + castResult.m_fraction = m_hitFraction; + btSphereShape pointShape(m_ccdSphereRadius); + btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, + ident,ident,castResult)) + { + if (m_hitFraction > castResult.m_fraction) + m_hitFraction = castResult.m_fraction; + } + + } + + }; + + + + + + if (triBody->getCollisionShape()->isConcave()) + { + btVector3 rayAabbMin = convexFromLocal.getOrigin(); + rayAabbMin.setMin(convexToLocal.getOrigin()); + btVector3 rayAabbMax = convexFromLocal.getOrigin(); + rayAabbMax.setMax(convexToLocal.getOrigin()); + btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); + rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); + + btScalar curHitFraction = btScalar(1.); //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, + convexbody->getCcdSweptSphereRadius(),curHitFraction); + + raycastCallback.m_hitFraction = convexbody->getHitFraction(); + + btCollisionObject* concavebody = triBody; + + btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); + + if (triangleMesh) + { + triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + } + + + + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) + { + convexbody->setHitFraction( raycastCallback.m_hitFraction); + return raycastCallback.m_hitFraction; + } + } + + return btScalar(1.); + +} diff --git a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index cddcf2cda..a6ea33717 100644 --- a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -1,153 +1,153 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H -#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -class btDispatcher; -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btSoftBody; -class btCollisionShape; - -#include "LinearMath/btHashMap.h" - -#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS - -struct btTriIndex -{ - int m_PartIdTriangleIndex; - class btCollisionShape* m_childShape; - - btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) - { - m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; - m_childShape = shape; - } - - int getTriangleIndex() const - { - // Get only the lower bits where the triangle index is stored - return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); - } - int getPartId() const - { - // Get only the highest bits where the part index is stored - return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); - } - int getUid() const - { - return m_PartIdTriangleIndex; - } -}; - - -///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called. -class btSoftBodyTriangleCallback : public btTriangleCallback -{ - btSoftBody* m_softBody; - btCollisionObject* m_triBody; - - btVector3 m_aabbMin; - btVector3 m_aabbMax ; - - btManifoldResult* m_resultOut; - - btDispatcher* m_dispatcher; - const btDispatcherInfo* m_dispatchInfoPtr; - btScalar m_collisionMarginTriangle; - - btHashMap,btTriIndex> m_shapeCache; - -public: - int m_triangleCount; - - // btPersistentManifold* m_manifoldPtr; - - btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); - - void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual ~btSoftBodyTriangleCallback(); - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - - void clearCache(); - - SIMD_FORCE_INLINE const btVector3& getAabbMin() const - { - return m_aabbMin; - } - SIMD_FORCE_INLINE const btVector3& getAabbMax() const - { - return m_aabbMax; - } - -}; - - - - -/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. -class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm -{ - - bool m_isSwapped; - - btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; - -public: - - btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); - - virtual ~btSoftBodyConcaveCollisionAlgorithm(); - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - void clearCache(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false); - } - }; - - struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); - return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true); - } - }; - -}; - -#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btSoftBody; +class btCollisionShape; + +#include "LinearMath/btHashMap.h" + +#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS + +struct btTriIndex +{ + int m_PartIdTriangleIndex; + class btCollisionShape* m_childShape; + + btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) + { + m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; + m_childShape = shape; + } + + int getTriangleIndex() const + { + // Get only the lower bits where the triangle index is stored + return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); + } + int getPartId() const + { + // Get only the highest bits where the part index is stored + return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); + } + int getUid() const + { + return m_PartIdTriangleIndex; + } +}; + + +///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called. +class btSoftBodyTriangleCallback : public btTriangleCallback +{ + btSoftBody* m_softBody; + btCollisionObject* m_triBody; + + btVector3 m_aabbMin; + btVector3 m_aabbMax ; + + btManifoldResult* m_resultOut; + + btDispatcher* m_dispatcher; + const btDispatcherInfo* m_dispatchInfoPtr; + btScalar m_collisionMarginTriangle; + + btHashMap,btTriIndex> m_shapeCache; + +public: + int m_triangleCount; + + // btPersistentManifold* m_manifoldPtr; + + btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); + + void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual ~btSoftBodyTriangleCallback(); + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + void clearCache(); + + SIMD_FORCE_INLINE const btVector3& getAabbMin() const + { + return m_aabbMin; + } + SIMD_FORCE_INLINE const btVector3& getAabbMax() const + { + return m_aabbMax; + } + +}; + + + + +/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. +class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm +{ + + bool m_isSwapped; + + btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; + +public: + + btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); + + virtual ~btSoftBodyConcaveCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + //we don't add any manifolds + } + + void clearCache(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); + return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); + return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true); + } + }; + +}; + +#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 8bc025819..6ab93c164 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1,859 +1,859 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBodyHelpers.cpp by Nathanael Presson - -#include "btSoftBodyInternals.h" -#include -#include -#include "btSoftBodyHelpers.h" -#include "LinearMath/btConvexHull.h" - -// -static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) -{ - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); -} - -// -static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) -{ - const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; - idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); - idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); - idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); - idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); - idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); - idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); -} - -// -static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) -{ - if(node) - { - if(node->isinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); - } - if(depth>=mindepth) - { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); - } - } -} - -// -template -static inline T sum(const btAlignedObjectArray& items) -{ - T v; - if(items.size()) - { - v=items[0]; - for(int i=1,ni=items.size();i -static inline void add(btAlignedObjectArray& items,const Q& value) -{ - for(int i=0,ni=items.size();i -static inline void mul(btAlignedObjectArray& items,const Q& value) -{ - for(int i=0,ni=items.size();i -static inline T average(const btAlignedObjectArray& items) -{ - const btScalar n=(btScalar)(items.size()>0?items.size():1); - return(sum(items)/n); -} - -// -static inline btScalar tetravolume(const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(dot(a,cross(b,c))); -} - -// -#if 0 -static btVector3 stresscolor(btScalar stress) -{ - static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; - static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; - static const btScalar one=1; - stress=btMax(0,btMin(1,stress))*ncolors; - const int sel=(int)stress; - const btScalar frc=stress-sel; - return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); -} -#endif - -// -void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) -{ - const btScalar scl=(btScalar)0.1; - const btScalar nscl=scl*5; - const btVector3 lcolor=btVector3(0,0,0); - const btVector3 ncolor=btVector3(1,1,1); - const btVector3 ccolor=btVector3(1,0,0); - int i,j,nj; - - /* Nodes */ - if(0!=(drawflags&fDrawFlags::Nodes)) - { - for(i=0;im_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); - idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); - idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); - } - } - /* Links */ - if(0!=(drawflags&fDrawFlags::Links)) - { - for(i=0;im_links.size();++i) - { - const btSoftBody::Link& l=psb->m_links[i]; - if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); - } - } - /* Normals */ - if(0!=(drawflags&fDrawFlags::Normals)) - { - for(i=0;im_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 d=n.m_n*nscl; - idraw->drawLine(n.m_x,n.m_x+d,ncolor); - idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); - } - } - /* Contacts */ - if(0!=(drawflags&fDrawFlags::Contacts)) - { - static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; - for(i=0;im_rcontacts.size();++i) - { - const btSoftBody::RContact& c=psb->m_rcontacts[i]; - const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* - (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); - const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); - const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); - idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); - idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); - idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); - } - } - /* Anchors */ - if(0!=(drawflags&fDrawFlags::Anchors)) - { - for(i=0;im_anchors.size();++i) - { - const btSoftBody::Anchor& a=psb->m_anchors[i]; - const btVector3 q=a.m_body->getWorldTransform()*a.m_local; - drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); - drawVertex(idraw,q,0.25,btVector3(0,1,0)); - idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); - } - for(i=0;im_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - if(n.m_im<=0) - { - drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); - } - } - } - /* Faces */ - if(0!=(drawflags&fDrawFlags::Faces)) - { - const btScalar scl=(btScalar)0.8; - const btScalar alp=(btScalar)1; - const btVector3 col(0,(btScalar)0.7,0); - for(i=0;im_faces.size();++i) - { - const btSoftBody::Face& f=psb->m_faces[i]; - if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; - const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; - const btVector3 c=(x[0]+x[1]+x[2])/3; - idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); - } - } - /* Clusters */ - if(0!=(drawflags&fDrawFlags::Clusters)) - { - srand(1806); - for(i=0;im_clusters.size();++i) - { - if(psb->m_clusters[i]->m_collide) - { - btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); - color=color.normalized()*0.75; - btAlignedObjectArray vertices; - vertices.resize(psb->m_clusters[i]->m_nodes.size()); - for(j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; - } - HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); - HullResult hres; - HullLibrary hlib; - hdsc.mMaxVertices=vertices.size(); - hlib.CreateConvexHull(hdsc,hres); - const btVector3 center=average(hres.m_OutputVertices); - add(hres.m_OutputVertices,-center); - mul(hres.m_OutputVertices,(btScalar)1); - add(hres.m_OutputVertices,center); - for(j=0;j<(int)hres.mNumFaces;++j) - { - const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; - idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); - } - /* Velocities */ -#if 0 - for(int j=0;jm_clusters[i].m_nodes.size();++j) - { - const btSoftBody::Cluster& c=psb->m_clusters[i]; - const btVector3 r=c.m_nodes[j]->m_x-c.m_com; - const btVector3 v=c.m_lv+cross(c.m_av,r); - idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); - } -#endif - /* Frame */ - btSoftBody::Cluster& c=*psb->m_clusters[i]; - idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); - idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); - idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); - } - } - /* Notes */ - if(0!=(drawflags&fDrawFlags::Notes)) - { - for(i=0;im_notes.size();++i) - { - const btSoftBody::Note& n=psb->m_notes[i]; - btVector3 p=n.m_offset; - for(int j=0;jm_x*n.m_coords[j]; - } - idraw->draw3dText(p,n.m_text); - } - } - /* Node tree */ - if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); - /* Face tree */ - if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); - /* Cluster tree */ - if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); - /* Joints */ - if(0!=(drawflags&fDrawFlags::Joints)) - { - for(i=0;im_joints.size();++i) - { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) - { - case btSoftBody::Joint::eType::Linear: - { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); - } - break; - case btSoftBody::Joint::eType::Angular: - { - const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); - } - } - } - } -} - -// -void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) -{ - for(int i=0;im_nodes.size();++i) - { - const btSoftBody::Node& n=psb->m_nodes[i]; - char text[2048]={0}; - char buff[1024]; - if(masses) - { - sprintf(buff," M(%.2f)",1/n.m_im); - strcat(text,buff); - } - if(areas) - { - sprintf(buff," A(%.2f)",n.m_area); - strcat(text,buff); - } - if(text[0]) idraw->draw3dText(n.m_x,text); - } -} - -// -void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); -} - -// -void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); -} - -// -void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) -{ - drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); -} - -// -void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) -{ - if(psb->m_pose.m_bframe) - { - static const btScalar ascl=10; - static const btScalar nscl=(btScalar)0.1; - const btVector3 com=psb->m_pose.m_com; - const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; - const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); - const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); - const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); - idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); - idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); - idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); - for(int i=0;im_pose.m_pos.size();++i) - { - const btVector3 x=com+trs*psb->m_pose.m_pos[i]; - drawVertex(idraw,x,nscl,btVector3(1,0,1)); - } - } -} - -// -btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) -{ - /* Create nodes */ - const int r=res+2; - btVector3* x=new btVector3[r]; - btScalar* m=new btScalar[r]; - int i; - - for(i=0;isetMass(0,0); - if(fixeds&2) psb->setMass(r-1,0); - delete[] x; - delete[] m; - /* Create links */ - for(i=1;iappendLink(i-1,i); - } - /* Finished */ - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) -{ -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; - int iy; - - for(iy=0;iysetMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - delete[] x; - delete[] m; - /* Create links and faces */ - for(iy=0;iyappendLink(idx,IDX(ix+1,iy)); - if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); - if(mdx&&mdy) - { - if((ix+iy)&1) - { - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); - psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); - if(gendiags) - { - psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); - } - } - else - { - psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); - psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); - if(gendiags) - { - psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); - } - } - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords) -{ - - /* - * - * corners: - * - * [0][0] corner00 ------- corner01 [resx][0] - * | | - * | | - * [0][resy] corner10 -------- corner11 [resx][resy] - * - * - * - * - * - * - * "fixedgs" map: - * - * corner00 --> +1 - * corner01 --> +2 - * corner10 --> +4 - * corner11 --> +8 - * upper middle --> +16 - * left middle --> +32 - * right middle --> +64 - * lower middle --> +128 - * center --> +256 - * - * - * tex_coords size (resx-1)*(resy-1)*12 - * - * - * - * SINGLE QUAD INTERNALS - * - * 1) btSoftBody's nodes and links, - * diagonal link is optional ("gendiags") - * - * - * node00 ------ node01 - * | . - * | . - * | . - * | . - * | . - * node10 node11 - * - * - * - * 2) Faces: - * two triangles, - * UV Coordinates (hier example for single quad) - * - * (0,1) (0,1) (1,1) - * 1 |\ 3 \-----| 2 - * | \ \ | - * | \ \ | - * | \ \ | - * | \ \ | - * 2 |-----\ 3 \| 1 - * (0,0) (1,0) (1,0) - * - * - * - * - * - * - */ - -#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) - /* Create nodes */ - if((resx<2)||(resy<2)) return(0); - const int rx=resx; - const int ry=resy; - const int tot=rx*ry; - btVector3* x=new btVector3[tot]; - btScalar* m=new btScalar[tot]; - - int iy; - - for(iy=0;iysetMass(IDX(0,0),0); - if(fixeds&2) psb->setMass(IDX(rx-1,0),0); - if(fixeds&4) psb->setMass(IDX(0,ry-1),0); - if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); - if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); - if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); - if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); - if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); - if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); - delete[] x; - delete[] m; - - - int z = 0; - /* Create links and faces */ - for(iy=0;iyappendLink(node00,node01); - if(mdy) psb->appendLink(node00,node10); - if(mdx&&mdy) - { - psb->appendFace(node00,node10,node11); - if (tex_coords) { - tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); - } - psb->appendFace(node11,node01,node00); - if (tex_coords) { - tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); - tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); - tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); - tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); - tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); - } - if (gendiags) psb->appendLink(node00,node11); - z += 12; - } - } - } - /* Finished */ -#undef IDX - return(psb); -} - -float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) -{ - - /* - * - * - * node00 --- node01 - * | | - * node10 --- node11 - * - * - * ID map: - * - * node00 s --> 0 - * node00 t --> 1 - * - * node01 s --> 3 - * node01 t --> 1 - * - * node10 s --> 0 - * node10 t --> 2 - * - * node11 s --> 3 - * node11 t --> 2 - * - * - */ - - float tc=0.0f; - if (id == 0) { - tc = (1.0f/((resx-1))*ix); - } - else if (id==1) { - tc = (1.0f/((resy-1))*(resy-1-iy)); - } - else if (id==2) { - tc = (1.0f/((resy-1))*(resy-1-iy-1)); - } - else if (id==3) { - tc = (1.0f/((resx-1))*(ix+1)); - } - return tc; -} -// -btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) -{ - struct Hammersley - { - static void Generate(btVector3* x,int n) - { - for(int i=0;i>=1) if(j&1) t+=p; - btScalar w=2*t-1; - btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; - btScalar s=btSqrt(1-w*w); - *x++=btVector3(s*btCos(a),s*btSin(a),w); - } - } - }; - btAlignedObjectArray vtx; - vtx.resize(3+res); - Hammersley::Generate(&vtx[0],vtx.size()); - for(int i=0;i chks; - btAlignedObjectArray vtx; - chks.resize(maxidx*maxidx,false); - vtx.resize(maxidx); - for(i=0,j=0,ni=maxidx*3;iappendLink(idx[j],idx[k]); - } - } -#undef IDX - psb->appendFace(idx[0],idx[1],idx[2]); - } - psb->randomizeConstraints(); - return(psb); -} - -// -btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices) -{ - HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); - HullResult hres; - HullLibrary hlib;/*??*/ - hdsc.mMaxVertices=nvertices; - hlib.CreateConvexHull(hdsc,hres); - btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, - &hres.m_OutputVertices[0],0); - for(int i=0;i<(int)hres.mNumFaces;++i) - { - const int idx[]={ hres.m_Indices[i*3+0], - hres.m_Indices[i*3+1], - hres.m_Indices[i*3+2]}; - if(idx[0]appendLink( idx[0],idx[1]); - if(idx[1]appendLink( idx[1],idx[2]); - if(idx[2]appendLink( idx[2],idx[0]); - psb->appendFace(idx[0],idx[1],idx[2]); - } - hlib.ReleaseResult(hres); - psb->randomizeConstraints(); - return(psb); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBodyHelpers.cpp by Nathanael Presson + +#include "btSoftBodyInternals.h" +#include +#include +#include "btSoftBodyHelpers.h" +#include "LinearMath/btConvexHull.h" + +// +static void drawVertex( btIDebugDraw* idraw, + const btVector3& x,btScalar s,const btVector3& c) +{ + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); +} + +// +static void drawBox( btIDebugDraw* idraw, + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) +{ + const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), + btVector3(maxs.x(),mins.y(),mins.z()), + btVector3(maxs.x(),maxs.y(),mins.z()), + btVector3(mins.x(),maxs.y(),mins.z()), + btVector3(mins.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),maxs.y(),maxs.z()), + btVector3(mins.x(),maxs.y(),maxs.z())}; + idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); + idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); + idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); + idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); + idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); + idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); +} + +// +static void drawTree( btIDebugDraw* idraw, + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) +{ + if(node) + { + if(node->isinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + } + if(depth>=mindepth) + { + const btScalar scl=(btScalar)(node->isinternal()?1:1); + const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; + const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; + drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + } + } +} + +// +template +static inline T sum(const btAlignedObjectArray& items) +{ + T v; + if(items.size()) + { + v=items[0]; + for(int i=1,ni=items.size();i +static inline void add(btAlignedObjectArray& items,const Q& value) +{ + for(int i=0,ni=items.size();i +static inline void mul(btAlignedObjectArray& items,const Q& value) +{ + for(int i=0,ni=items.size();i +static inline T average(const btAlignedObjectArray& items) +{ + const btScalar n=(btScalar)(items.size()>0?items.size():1); + return(sum(items)/n); +} + +// +static inline btScalar tetravolume(const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) +{ + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 c=x3-x0; + return(dot(a,cross(b,c))); +} + +// +#if 0 +static btVector3 stresscolor(btScalar stress) +{ + static const btVector3 spectrum[]= { btVector3(1,0,1), + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0)}; + static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; + static const btScalar one=1; + stress=btMax(0,btMin(1,stress))*ncolors; + const int sel=(int)stress; + const btScalar frc=stress-sel; + return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); +} +#endif + +// +void btSoftBodyHelpers::Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags) +{ + const btScalar scl=(btScalar)0.1; + const btScalar nscl=scl*5; + const btVector3 lcolor=btVector3(0,0,0); + const btVector3 ncolor=btVector3(1,1,1); + const btVector3 ccolor=btVector3(1,0,0); + int i,j,nj; + + /* Nodes */ + if(0!=(drawflags&fDrawFlags::Nodes)) + { + for(i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0)); + idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0)); + idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1)); + } + } + /* Links */ + if(0!=(drawflags&fDrawFlags::Links)) + { + for(i=0;im_links.size();++i) + { + const btSoftBody::Link& l=psb->m_links[i]; + if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor); + } + } + /* Normals */ + if(0!=(drawflags&fDrawFlags::Normals)) + { + for(i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 d=n.m_n*nscl; + idraw->drawLine(n.m_x,n.m_x+d,ncolor); + idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5); + } + } + /* Contacts */ + if(0!=(drawflags&fDrawFlags::Contacts)) + { + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + for(i=0;im_rcontacts.size();++i) + { + const btSoftBody::RContact& c=psb->m_rcontacts[i]; + const btVector3 o= c.m_node->m_x-c.m_cti.m_normal* + (dot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset); + const btVector3 x=cross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized(); + const btVector3 y=cross(x,c.m_cti.m_normal).normalized(); + idraw->drawLine(o-x*nscl,o+x*nscl,ccolor); + idraw->drawLine(o-y*nscl,o+y*nscl,ccolor); + idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0)); + } + } + /* Anchors */ + if(0!=(drawflags&fDrawFlags::Anchors)) + { + for(i=0;im_anchors.size();++i) + { + const btSoftBody::Anchor& a=psb->m_anchors[i]; + const btVector3 q=a.m_body->getWorldTransform()*a.m_local; + drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0)); + drawVertex(idraw,q,0.25,btVector3(0,1,0)); + idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1)); + } + for(i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + if(n.m_im<=0) + { + drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0)); + } + } + } + /* Faces */ + if(0!=(drawflags&fDrawFlags::Faces)) + { + const btScalar scl=(btScalar)0.8; + const btScalar alp=(btScalar)1; + const btVector3 col(0,(btScalar)0.7,0); + for(i=0;im_faces.size();++i) + { + const btSoftBody::Face& f=psb->m_faces[i]; + if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue; + const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; + const btVector3 c=(x[0]+x[1]+x[2])/3; + idraw->drawTriangle((x[0]-c)*scl+c, + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); + } + } + /* Clusters */ + if(0!=(drawflags&fDrawFlags::Clusters)) + { + srand(1806); + for(i=0;im_clusters.size();++i) + { + if(psb->m_clusters[i]->m_collide) + { + btVector3 color( rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX); + color=color.normalized()*0.75; + btAlignedObjectArray vertices; + vertices.resize(psb->m_clusters[i]->m_nodes.size()); + for(j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; + } + HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); + HullResult hres; + HullLibrary hlib; + hdsc.mMaxVertices=vertices.size(); + hlib.CreateConvexHull(hdsc,hres); + const btVector3 center=average(hres.m_OutputVertices); + add(hres.m_OutputVertices,-center); + mul(hres.m_OutputVertices,(btScalar)1); + add(hres.m_OutputVertices,center); + for(j=0;j<(int)hres.mNumFaces;++j) + { + const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; + idraw->drawTriangle(hres.m_OutputVertices[idx[0]], + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color,1); + } + hlib.ReleaseResult(hres); + } + /* Velocities */ +#if 0 + for(int j=0;jm_clusters[i].m_nodes.size();++j) + { + const btSoftBody::Cluster& c=psb->m_clusters[i]; + const btVector3 r=c.m_nodes[j]->m_x-c.m_com; + const btVector3 v=c.m_lv+cross(c.m_av,r); + idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); + } +#endif + /* Frame */ + btSoftBody::Cluster& c=*psb->m_clusters[i]; + idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); + idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); + idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); + } + } + /* Notes */ + if(0!=(drawflags&fDrawFlags::Notes)) + { + for(i=0;im_notes.size();++i) + { + const btSoftBody::Note& n=psb->m_notes[i]; + btVector3 p=n.m_offset; + for(int j=0;jm_x*n.m_coords[j]; + } + idraw->draw3dText(p,n.m_text); + } + } + /* Node tree */ + if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw); + /* Face tree */ + if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw); + /* Cluster tree */ + if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw); + /* Joints */ + if(0!=(drawflags&fDrawFlags::Joints)) + { + for(i=0;im_joints.size();++i) + { + const btSoftBody::Joint* pj=psb->m_joints[i]; + switch(pj->Type()) + { + case btSoftBody::Joint::eType::Linear: + { + const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; + const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); + drawVertex(idraw,a0,0.25,btVector3(1,1,0)); + drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + } + break; + case btSoftBody::Joint::eType::Angular: + { + const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; + idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); + idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); + idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); + idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + } + } + } + } +} + +// +void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) +{ + for(int i=0;im_nodes.size();++i) + { + const btSoftBody::Node& n=psb->m_nodes[i]; + char text[2048]={0}; + char buff[1024]; + if(masses) + { + sprintf(buff," M(%.2f)",1/n.m_im); + strcat(text,buff); + } + if(areas) + { + sprintf(buff," A(%.2f)",n.m_area); + strcat(text,buff); + } + if(text[0]) idraw->draw3dText(n.m_x,text); + } +} + +// +void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) +{ + drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); +} + +// +void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) +{ + drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); +} + +// +void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth, + int maxdepth) +{ + drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); +} + +// +void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, + btIDebugDraw* idraw) +{ + if(psb->m_pose.m_bframe) + { + static const btScalar ascl=10; + static const btScalar nscl=(btScalar)0.1; + const btVector3 com=psb->m_pose.m_com; + const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl; + const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized(); + const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized(); + const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized(); + idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0)); + idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0)); + idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1)); + for(int i=0;im_pose.m_pos.size();++i) + { + const btVector3 x=com+trs*psb->m_pose.m_pos[i]; + drawVertex(idraw,x,nscl,btVector3(1,0,1)); + } + } +} + +// +btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, + const btVector3& to, + int res, + int fixeds) +{ + /* Create nodes */ + const int r=res+2; + btVector3* x=new btVector3[r]; + btScalar* m=new btScalar[r]; + int i; + + for(i=0;isetMass(0,0); + if(fixeds&2) psb->setMass(r-1,0); + delete[] x; + delete[] m; + /* Create links */ + for(i=1;iappendLink(i-1,i); + } + /* Finished */ + return(psb); +} + +// +btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) +{ +#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) + /* Create nodes */ + if((resx<2)||(resy<2)) return(0); + const int rx=resx; + const int ry=resy; + const int tot=rx*ry; + btVector3* x=new btVector3[tot]; + btScalar* m=new btScalar[tot]; + int iy; + + for(iy=0;iysetMass(IDX(0,0),0); + if(fixeds&2) psb->setMass(IDX(rx-1,0),0); + if(fixeds&4) psb->setMass(IDX(0,ry-1),0); + if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); + delete[] x; + delete[] m; + /* Create links and faces */ + for(iy=0;iyappendLink(idx,IDX(ix+1,iy)); + if(mdy) psb->appendLink(idx,IDX(ix,iy+1)); + if(mdx&&mdy) + { + if((ix+iy)&1) + { + psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1)); + psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1)); + if(gendiags) + { + psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1)); + } + } + else + { + psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy)); + psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1)); + if(gendiags) + { + psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1)); + } + } + } + } + } + /* Finished */ +#undef IDX + return(psb); +} + +// +btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords) +{ + + /* + * + * corners: + * + * [0][0] corner00 ------- corner01 [resx][0] + * | | + * | | + * [0][resy] corner10 -------- corner11 [resx][resy] + * + * + * + * + * + * + * "fixedgs" map: + * + * corner00 --> +1 + * corner01 --> +2 + * corner10 --> +4 + * corner11 --> +8 + * upper middle --> +16 + * left middle --> +32 + * right middle --> +64 + * lower middle --> +128 + * center --> +256 + * + * + * tex_coords size (resx-1)*(resy-1)*12 + * + * + * + * SINGLE QUAD INTERNALS + * + * 1) btSoftBody's nodes and links, + * diagonal link is optional ("gendiags") + * + * + * node00 ------ node01 + * | . + * | . + * | . + * | . + * | . + * node10 node11 + * + * + * + * 2) Faces: + * two triangles, + * UV Coordinates (hier example for single quad) + * + * (0,1) (0,1) (1,1) + * 1 |\ 3 \-----| 2 + * | \ \ | + * | \ \ | + * | \ \ | + * | \ \ | + * 2 |-----\ 3 \| 1 + * (0,0) (1,0) (1,0) + * + * + * + * + * + * + */ + +#define IDX(_x_,_y_) ((_y_)*rx+(_x_)) + /* Create nodes */ + if((resx<2)||(resy<2)) return(0); + const int rx=resx; + const int ry=resy; + const int tot=rx*ry; + btVector3* x=new btVector3[tot]; + btScalar* m=new btScalar[tot]; + + int iy; + + for(iy=0;iysetMass(IDX(0,0),0); + if(fixeds&2) psb->setMass(IDX(rx-1,0),0); + if(fixeds&4) psb->setMass(IDX(0,ry-1),0); + if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0); + if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0); + if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0); + if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0); + if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0); + if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0); + delete[] x; + delete[] m; + + + int z = 0; + /* Create links and faces */ + for(iy=0;iyappendLink(node00,node01); + if(mdy) psb->appendLink(node00,node10); + if(mdx&&mdy) + { + psb->appendFace(node00,node10,node11); + if (tex_coords) { + tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2); + } + psb->appendFace(node11,node01,node00); + if (tex_coords) { + tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2); + tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3); + tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1); + tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0); + tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1); + } + if (gendiags) psb->appendLink(node00,node11); + z += 12; + } + } + } + /* Finished */ +#undef IDX + return(psb); +} + +float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) +{ + + /* + * + * + * node00 --- node01 + * | | + * node10 --- node11 + * + * + * ID map: + * + * node00 s --> 0 + * node00 t --> 1 + * + * node01 s --> 3 + * node01 t --> 1 + * + * node10 s --> 0 + * node10 t --> 2 + * + * node11 s --> 3 + * node11 t --> 2 + * + * + */ + + float tc=0.0f; + if (id == 0) { + tc = (1.0f/((resx-1))*ix); + } + else if (id==1) { + tc = (1.0f/((resy-1))*(resy-1-iy)); + } + else if (id==2) { + tc = (1.0f/((resy-1))*(resy-1-iy-1)); + } + else if (id==3) { + tc = (1.0f/((resx-1))*(ix+1)); + } + return tc; +} +// +btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, + const btVector3& radius, + int res) +{ + struct Hammersley + { + static void Generate(btVector3* x,int n) + { + for(int i=0;i>=1) if(j&1) t+=p; + btScalar w=2*t-1; + btScalar a=(SIMD_PI+2*i*SIMD_PI)/n; + btScalar s=btSqrt(1-w*w); + *x++=btVector3(s*btCos(a),s*btSin(a),w); + } + } + }; + btAlignedObjectArray vtx; + vtx.resize(3+res); + Hammersley::Generate(&vtx[0],vtx.size()); + for(int i=0;i chks; + btAlignedObjectArray vtx; + chks.resize(maxidx*maxidx,false); + vtx.resize(maxidx); + for(i=0,j=0,ni=maxidx*3;iappendLink(idx[j],idx[k]); + } + } +#undef IDX + psb->appendFace(idx[0],idx[1],idx[2]); + } + psb->randomizeConstraints(); + return(psb); +} + +// +btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, + int nvertices) +{ + HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); + HullResult hres; + HullLibrary hlib;/*??*/ + hdsc.mMaxVertices=nvertices; + hlib.CreateConvexHull(hdsc,hres); + btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices, + &hres.m_OutputVertices[0],0); + for(int i=0;i<(int)hres.mNumFaces;++i) + { + const int idx[]={ hres.m_Indices[i*3+0], + hres.m_Indices[i*3+1], + hres.m_Indices[i*3+2]}; + if(idx[0]appendLink( idx[0],idx[1]); + if(idx[1]appendLink( idx[1],idx[2]); + if(idx[2]appendLink( idx[2],idx[0]); + psb->appendFace(idx[0],idx[1],idx[2]); + } + hlib.ReleaseResult(hres); + psb->randomizeConstraints(); + return(psb); +} diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index 78da9f503..0e3b50397 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -1,119 +1,119 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SOFT_BODY_HELPERS_H -#define SOFT_BODY_HELPERS_H - -#include "btSoftBody.h" - -// -// Helpers -// - -/* fDrawFlags */ -struct fDrawFlags { enum _ { - Nodes = 0x0001, - Links = 0x0002, - Faces = 0x0004, - Tetras = 0x0008, - Normals = 0x0010, - Contacts = 0x0020, - Anchors = 0x0040, - Notes = 0x0080, - Clusters = 0x0100, - NodeTree = 0x0200, - FaceTree = 0x0400, - ClusterTree = 0x0800, - Joints = 0x1000, - /* presets */ - Std = Links+Faces+Tetras+Anchors+Notes+Joints, - StdTetra = Std-Faces+Tetras -};}; - -struct btSoftBodyHelpers -{ - /* Draw body */ - static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); - /* Draw body infos */ - static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); - /* Draw node tree */ - static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw face tree */ - static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw cluster tree */ - static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); - /* Draw rigid frame */ - static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); - /* Create a rope */ - static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); - /* Create a patch */ - static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); - /* Create a patch with UV Texture Coordinates */ - static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords=0); - static float CalculateUV(int resx,int resy,int ix,int iy,int id); - /* Create an ellipsoid */ - static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); - /* Create from trimesh */ - static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles); - /* Create from convex-hull */ - static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices); -}; - -#endif //SOFT_BODY_HELPERS_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SOFT_BODY_HELPERS_H +#define SOFT_BODY_HELPERS_H + +#include "btSoftBody.h" + +// +// Helpers +// + +/* fDrawFlags */ +struct fDrawFlags { enum _ { + Nodes = 0x0001, + Links = 0x0002, + Faces = 0x0004, + Tetras = 0x0008, + Normals = 0x0010, + Contacts = 0x0020, + Anchors = 0x0040, + Notes = 0x0080, + Clusters = 0x0100, + NodeTree = 0x0200, + FaceTree = 0x0400, + ClusterTree = 0x0800, + Joints = 0x1000, + /* presets */ + Std = Links+Faces+Tetras+Anchors+Notes+Joints, + StdTetra = Std-Faces+Tetras +};}; + +struct btSoftBodyHelpers +{ + /* Draw body */ + static void Draw( btSoftBody* psb, + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); + /* Draw body infos */ + static void DrawInfos( btSoftBody* psb, + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); + /* Draw node tree */ + static void DrawNodeTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); + /* Draw face tree */ + static void DrawFaceTree( btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); + /* Draw cluster tree */ + static void DrawClusterTree(btSoftBody* psb, + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); + /* Draw rigid frame */ + static void DrawFrame( btSoftBody* psb, + btIDebugDraw* idraw); + /* Create a rope */ + static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, + const btVector3& from, + const btVector3& to, + int res, + int fixeds); + /* Create a patch */ + static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); + /* Create a patch with UV Texture Coordinates */ + static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords=0); + static float CalculateUV(int resx,int resy,int ix,int iy,int id); + /* Create an ellipsoid */ + static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, + const btVector3& center, + const btVector3& radius, + int res); + /* Create from trimesh */ + static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, + const btScalar* vertices, + const int* triangles, + int ntriangles); + /* Create from convex-hull */ + static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, + const btVector3* vertices, + int nvertices); +}; + +#endif //SOFT_BODY_HELPERS_H diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 3ea095379..dc8dc398f 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1,897 +1,897 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSoftBody implementation by Nathanael Presson - -#ifndef _BT_SOFT_BODY_INTERNALS_H -#define _BT_SOFT_BODY_INTERNALS_H - -#include "btSoftBody.h" - -#include "LinearMath/btQuickprof.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - -// -// btSymMatrix -// -template -struct btSymMatrix -{ - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } - void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } - int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; - int dim; -}; - -// -// btSoftBodyCollisionShape -// -class btSoftBodyCollisionShape : public btConcaveShape -{ -public: - btSoftBody* m_body; - - btSoftBodyCollisionShape(btSoftBody* backptr) - { - m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; - m_body=backptr; - } - - virtual ~btSoftBodyCollisionShape() - { - - } - - void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const - { - //not yet - btAssert(0); - } - - ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - { - /* t should be identity, but better be safe than...fast? */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) - { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); - } - } - - - virtual void setLocalScaling(const btVector3& /*scaling*/) - { - ///na - } - virtual const btVector3& getLocalScaling() const - { - static const btVector3 dummy(1,1,1); - return dummy; - } - virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const - { - ///not yet - btAssert(0); - } - virtual const char* getName()const - { - return "SoftBody"; - } - -}; - -// -// btSoftClusterCollisionShape -// -class btSoftClusterCollisionShape : public btConvexInternalShape -{ -public: - const btSoftBody::Cluster* m_cluster; - - btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - - virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { - btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; - btScalar d=dot(vec,n[0]->m_x); - int j=0; - for(int i=1,ni=m_cluster->m_nodes.size();im_x); - if(k>d) { d=k;j=i; } - } - return(n[j]->m_x); - } - virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { - return(localGetSupportingVertex(vec)); - } - //notice that the vectors should be unit length - virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const - {} - - - virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const - {} - - virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const - {} - - virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - - //debugging - virtual const char* getName()const {return "SOFTCLUSTER";} - - virtual void setMargin(btScalar margin) - { - btConvexInternalShape::setMargin(margin); - } - virtual btScalar getMargin() const - { - return getMargin(); - } -}; - -// -// Inline's -// - -// -template -static inline void ZeroInitialize(T& value) -{ - static const T zerodummy; - value=zerodummy; -} -// -template -static inline bool CompLess(const T& a,const T& b) -{ return(a -static inline bool CompGreater(const T& a,const T& b) -{ return(a>b); } -// -template -static inline T Lerp(const T& a,const T& b,btScalar t) -{ return(a+(b-a)*t); } -// -template -static inline T InvLerp(const T& a,const T& b,btScalar t) -{ return((b+a*t-b*t)/(a*b)); } -// -static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) -{ - btMatrix3x3 r; - r[0]=Lerp(a[0],b[0],t); - r[1]=Lerp(a[1],b[1],t); - r[2]=Lerp(a[2],b[2],t); - return(r); -} -// -static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) -{ - const btScalar sql=v.length2(); - if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); - else - return(v); -} -// -template -static inline T Clamp(const T& x,const T& l,const T& h) -{ return(xh?h:x); } -// -template -static inline T Sq(const T& x) -{ return(x*x); } -// -template -static inline T Cube(const T& x) -{ return(x*x*x); } -// -template -static inline T Sign(const T& x) -{ return((T)(x<0?-1:+1)); } -// -template -static inline bool SameSign(const T& x,const T& y) -{ return((x*y)>0); } -// -static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) -{ - const btVector3 d=x-y; - return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); -} -// -static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) -{ - const btScalar xx=a.x()*a.x(); - const btScalar yy=a.y()*a.y(); - const btScalar zz=a.z()*a.z(); - const btScalar xy=a.x()*a.y(); - const btScalar yz=a.y()*a.z(); - const btScalar zx=a.z()*a.x(); - btMatrix3x3 m; - m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); - m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); - m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); - return(m); -} -// -static inline btMatrix3x3 Cross(const btVector3& v) -{ - btMatrix3x3 m; - m[0]=btVector3(0,-v.z(),+v.y()); - m[1]=btVector3(+v.z(),0,-v.x()); - m[2]=btVector3(-v.y(),+v.x(),0); - return(m); -} -// -static inline btMatrix3x3 Diagonal(btScalar x) -{ - btMatrix3x3 m; - m[0]=btVector3(x,0,0); - m[1]=btVector3(0,x,0); - m[2]=btVector3(0,0,x); - return(m); -} -// -static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]+b[i]; - return(r); -} -// -static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]-b[i]; - return(r); -} -// -static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) -{ - btMatrix3x3 r; - for(int i=0;i<3;++i) r[i]=a[i]*b; - return(r); -} -// -static inline void Orthogonalize(btMatrix3x3& m) -{ - m[2]=cross(m[0],m[1]).normalized(); - m[1]=cross(m[2],m[0]).normalized(); - m[0]=cross(m[1],m[2]).normalized(); -} -// -static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) -{ - const btMatrix3x3 cr=Cross(r); - return(Sub(Diagonal(im),cr*iwi*cr)); -} - -// -static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) -{ - return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); -} - -// -static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) -{ - return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); -} - -// -static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) -{ - return(Add(iia,iib).inverse()); -} - -// -static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) -{ - return(a*dot(v,a)); -} -// -static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) -{ - return(v-ProjectOnAxis(v,a)); -} - -// -static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) -{ - const btVector3 d=b-a; - const btScalar m2=d.length2(); - if(m2>SIMD_EPSILON) - { - const btScalar t=Clamp(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2SIMD_EPSILON) - { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k20)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) - { - prj=p; - sqd=k2; - } - else - { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); - } - } - } -} - -// -template -static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) -{ - return(a*coord.x()+b*coord.y()+c*coord.z()); -} -// -static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) -{ - const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; - const btScalar isum=1/(w[0]+w[1]+w[2]); - return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); -} - -// -static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) -{ - btScalar span[2]={0,1}; - btScalar values[2]={fn->Eval(a),fn->Eval(b)}; - if(values[0]>values[1]) - { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); - } - if(values[0]>-accuracy) return(-1); - if(values[1]<+accuracy) return(-1); - for(int i=0;iEval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)SIMD_EPSILON) - return(v/l); - else - return(btVector3(0,0,0)); -} - -// -static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, - btScalar margin) -{ - const btVector3* pts[]={ &f.m_n[0]->m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; - btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); - vol.Expand(btVector3(margin,margin,margin)); - return(vol); -} - -// -static inline btVector3 CenterOf( const btSoftBody::Face& f) -{ - return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); -} - -// -static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 cr=cross(a,b); - const btScalar area=cr.length(); - return(area); -} - -// -static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) -{ - const btVector3 a=x1-x0; - const btVector3 b=x2-x0; - const btVector3 c=x3-x0; - return(dot(a,cross(b,c))); -} - -// -static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) -{ - medium.m_velocity = btVector3(0,0,0); - medium.m_pressure = 0; - medium.m_density = wfi->air_density; - if(wfi->water_density>0) - { - const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset); - if(depth>0) - { - medium.m_density = wfi->water_density; - medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); - } - } -} - -// -static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) -{ - const btScalar dtim=dt*n.m_im; - if((f*dtim).length2()>n.m_v.length2()) - {/* Clamp */ - n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; - } - else - {/* Apply */ - n.m_f+=f; - } -} - -// -static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) -{ - if((a==ma)&&(b==mb)) return(0); - if((a==mb)&&(b==ma)) return(1); - return(-1); -} - -// -// btEigen : Extract eigen system, -// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html -// outputs are NOT sorted. -// -struct btEigen -{ - static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) - { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])accuracy) - { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ - { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); - } else break; - } else break; - } while((++iterations)accuracy) det=ndet; else break; - } - /* Final orthogonalization */ - Orthogonalize(q); - /* Compute 'S' */ - s=q.transpose()*m; - } - else - { - q.setIdentity(); - s.setIdentity(); - } - return(i); -} - -// -// btSoftColliders -// -struct btSoftColliders -{ - // - // ClusterBase - // - struct ClusterBase : btDbvt::ICollide - { - btScalar erp; - btScalar idt; - btScalar margin; - btScalar friction; - btScalar threshold; - ClusterBase() - { - erp =(btScalar)1; - idt =0; - margin =0; - friction =0; - threshold =(btScalar)0; - } - bool SolveContact( const btGjkEpaSolver2::sResults& res, - btSoftBody::Body ba,btSoftBody::Body bb, - btSoftBody::CJoint& joint) - { - if(res.distancedata; - btSoftClusterCollisionShape cshape(cluster); - const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,m_colObj->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,m_colObj,joint))//prb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(m_colObj->isStaticOrKinematicObject()) - { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; - } - else - { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; - } - } - } - } - void Process(btSoftBody* ps,btCollisionObject* colOb) - { - psb = ps; - m_colObj = colOb; - idt = ps->m_sst.isdt; - margin = m_colObj->getCollisionShape()->getMargin()+ - m_colObj->getCollisionShape()->getMargin(); - friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*margin); - ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); - } - }; - // - // CollideCL_SS - // - struct CollideCL_SS : ClusterBase - { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) - { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) - { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) - { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; - } - } - } - void Process(btSoftBody* psa,btSoftBody* psb) - { - idt = psa->m_sst.isdt; - margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); - } - }; - // - // CollideSDF_RS - // - struct CollideSDF_RS : btDbvt::ICollide - { - void Process(const btDbvtNode* leaf) - { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); - } - void DoNode(btSoftBody::Node& n) const - { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(m_colObj1,n.m_x,m,c.m_cti)) - { - const btScalar ima=n.m_im; - const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; - const btScalar ms=ima+imb; - if(ms>0) - { - const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); - const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } - } - } - btSoftBody* psb; - btCollisionObject* m_colObj1; - btRigidBody* m_rigidBody; - btScalar dynmargin; - btScalar stamargin; - }; - // - // CollideVF_SS - // - struct CollideVF_SS : btDbvt::ICollide - { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) - { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) - { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) - { - mb=0; - } - const btScalar ms=ma+mb; - if(ms>0) - { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); - } - } - } - btSoftBody* psb[2]; - btScalar mrg; - }; -}; - -#endif //_BT_SOFT_BODY_INTERNALS_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSoftBody implementation by Nathanael Presson + +#ifndef _BT_SOFT_BODY_INTERNALS_H +#define _BT_SOFT_BODY_INTERNALS_H + +#include "btSoftBody.h" + +#include "LinearMath/btQuickprof.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" + +// +// btSymMatrix +// +template +struct btSymMatrix +{ + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } + void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } + int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; + int dim; +}; + +// +// btSoftBodyCollisionShape +// +class btSoftBodyCollisionShape : public btConcaveShape +{ +public: + btSoftBody* m_body; + + btSoftBodyCollisionShape(btSoftBody* backptr) + { + m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; + m_body=backptr; + } + + virtual ~btSoftBodyCollisionShape() + { + + } + + void processAllTriangles(btTriangleCallback* /*callback*/,const btVector3& /*aabbMin*/,const btVector3& /*aabbMax*/) const + { + //not yet + btAssert(0); + } + + ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + /* t should be identity, but better be safe than...fast? */ + const btVector3 mins=m_body->m_bounds[0]; + const btVector3 maxs=m_body->m_bounds[1]; + const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),maxs.y(),maxs.z()), + t*btVector3(mins.x(),maxs.y(),maxs.z())}; + aabbMin=aabbMax=crns[0]; + for(int i=1;i<8;++i) + { + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); + } + } + + + virtual void setLocalScaling(const btVector3& /*scaling*/) + { + ///na + } + virtual const btVector3& getLocalScaling() const + { + static const btVector3 dummy(1,1,1); + return dummy; + } + virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const + { + ///not yet + btAssert(0); + } + virtual const char* getName()const + { + return "SoftBody"; + } + +}; + +// +// btSoftClusterCollisionShape +// +class btSoftClusterCollisionShape : public btConvexInternalShape +{ +public: + const btSoftBody::Cluster* m_cluster; + + btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } + + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + { + btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; + btScalar d=dot(vec,n[0]->m_x); + int j=0; + for(int i=1,ni=m_cluster->m_nodes.size();im_x); + if(k>d) { d=k;j=i; } + } + return(n[j]->m_x); + } + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + { + return(localGetSupportingVertex(vec)); + } + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + {} + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const + {} + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + {} + + virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } + + //debugging + virtual const char* getName()const {return "SOFTCLUSTER";} + + virtual void setMargin(btScalar margin) + { + btConvexInternalShape::setMargin(margin); + } + virtual btScalar getMargin() const + { + return getMargin(); + } +}; + +// +// Inline's +// + +// +template +static inline void ZeroInitialize(T& value) +{ + static const T zerodummy; + value=zerodummy; +} +// +template +static inline bool CompLess(const T& a,const T& b) +{ return(a +static inline bool CompGreater(const T& a,const T& b) +{ return(a>b); } +// +template +static inline T Lerp(const T& a,const T& b,btScalar t) +{ return(a+(b-a)*t); } +// +template +static inline T InvLerp(const T& a,const T& b,btScalar t) +{ return((b+a*t-b*t)/(a*b)); } +// +static inline btMatrix3x3 Lerp( const btMatrix3x3& a, + const btMatrix3x3& b, + btScalar t) +{ + btMatrix3x3 r; + r[0]=Lerp(a[0],b[0],t); + r[1]=Lerp(a[1],b[1],t); + r[2]=Lerp(a[2],b[2],t); + return(r); +} +// +static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) +{ + const btScalar sql=v.length2(); + if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); + else + return(v); +} +// +template +static inline T Clamp(const T& x,const T& l,const T& h) +{ return(xh?h:x); } +// +template +static inline T Sq(const T& x) +{ return(x*x); } +// +template +static inline T Cube(const T& x) +{ return(x*x*x); } +// +template +static inline T Sign(const T& x) +{ return((T)(x<0?-1:+1)); } +// +template +static inline bool SameSign(const T& x,const T& y) +{ return((x*y)>0); } +// +static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) +{ + const btVector3 d=x-y; + return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); +} +// +static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) +{ + const btScalar xx=a.x()*a.x(); + const btScalar yy=a.y()*a.y(); + const btScalar zz=a.z()*a.z(); + const btScalar xy=a.x()*a.y(); + const btScalar yz=a.y()*a.z(); + const btScalar zx=a.z()*a.x(); + btMatrix3x3 m; + m[0]=btVector3(1-xx+xx*s,xy*s-xy,zx*s-zx); + m[1]=btVector3(xy*s-xy,1-yy+yy*s,yz*s-yz); + m[2]=btVector3(zx*s-zx,yz*s-yz,1-zz+zz*s); + return(m); +} +// +static inline btMatrix3x3 Cross(const btVector3& v) +{ + btMatrix3x3 m; + m[0]=btVector3(0,-v.z(),+v.y()); + m[1]=btVector3(+v.z(),0,-v.x()); + m[2]=btVector3(-v.y(),+v.x(),0); + return(m); +} +// +static inline btMatrix3x3 Diagonal(btScalar x) +{ + btMatrix3x3 m; + m[0]=btVector3(x,0,0); + m[1]=btVector3(0,x,0); + m[2]=btVector3(0,0,x); + return(m); +} +// +static inline btMatrix3x3 Add(const btMatrix3x3& a, + const btMatrix3x3& b) +{ + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]+b[i]; + return(r); +} +// +static inline btMatrix3x3 Sub(const btMatrix3x3& a, + const btMatrix3x3& b) +{ + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]-b[i]; + return(r); +} +// +static inline btMatrix3x3 Mul(const btMatrix3x3& a, + btScalar b) +{ + btMatrix3x3 r; + for(int i=0;i<3;++i) r[i]=a[i]*b; + return(r); +} +// +static inline void Orthogonalize(btMatrix3x3& m) +{ + m[2]=cross(m[0],m[1]).normalized(); + m[1]=cross(m[2],m[0]).normalized(); + m[0]=cross(m[1],m[2]).normalized(); +} +// +static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) +{ + const btMatrix3x3 cr=Cross(r); + return(Sub(Diagonal(im),cr*iwi*cr)); +} + +// +static inline btMatrix3x3 ImpulseMatrix( btScalar dt, + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ + return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); +} + +// +static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) +{ + return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); +} + +// +static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, + const btMatrix3x3& iib) +{ + return(Add(iia,iib).inverse()); +} + +// +static inline btVector3 ProjectOnAxis( const btVector3& v, + const btVector3& a) +{ + return(a*dot(v,a)); +} +// +static inline btVector3 ProjectOnPlane( const btVector3& v, + const btVector3& a) +{ + return(v-ProjectOnAxis(v,a)); +} + +// +static inline void ProjectOrigin( const btVector3& a, + const btVector3& b, + btVector3& prj, + btScalar& sqd) +{ + const btVector3 d=b-a; + const btScalar m2=d.length2(); + if(m2>SIMD_EPSILON) + { + const btScalar t=Clamp(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2SIMD_EPSILON) + { + const btVector3 n=q/btSqrt(m2); + const btScalar k=dot(a,n); + const btScalar k2=k*k; + if(k20)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) + { + prj=p; + sqd=k2; + } + else + { + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); + } + } + } +} + +// +template +static inline T BaryEval( const T& a, + const T& b, + const T& c, + const btVector3& coord) +{ + return(a*coord.x()+b*coord.y()+c*coord.z()); +} +// +static inline btVector3 BaryCoord( const btVector3& a, + const btVector3& b, + const btVector3& c, + const btVector3& p) +{ + const btScalar w[]={ cross(a-p,b-p).length(), + cross(b-p,c-p).length(), + cross(c-p,a-p).length()}; + const btScalar isum=1/(w[0]+w[1]+w[2]); + return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); +} + +// +static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) +{ + btScalar span[2]={0,1}; + btScalar values[2]={fn->Eval(a),fn->Eval(b)}; + if(values[0]>values[1]) + { + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); + } + if(values[0]>-accuracy) return(-1); + if(values[1]<+accuracy) return(-1); + for(int i=0;iEval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)SIMD_EPSILON) + return(v/l); + else + return(btVector3(0,0,0)); +} + +// +static inline btDbvtVolume VolumeOf( const btSoftBody::Face& f, + btScalar margin) +{ + const btVector3* pts[]={ &f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); + vol.Expand(btVector3(margin,margin,margin)); + return(vol); +} + +// +static inline btVector3 CenterOf( const btSoftBody::Face& f) +{ + return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); +} + +// +static inline btScalar AreaOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2) +{ + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 cr=cross(a,b); + const btScalar area=cr.length(); + return(area); +} + +// +static inline btScalar VolumeOf( const btVector3& x0, + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) +{ + const btVector3 a=x1-x0; + const btVector3 b=x2-x0; + const btVector3 c=x3-x0; + return(dot(a,cross(b,c))); +} + +// +static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, + const btVector3& x, + btSoftBody::sMedium& medium) +{ + medium.m_velocity = btVector3(0,0,0); + medium.m_pressure = 0; + medium.m_density = wfi->air_density; + if(wfi->water_density>0) + { + const btScalar depth=-(dot(x,wfi->water_normal)+wfi->water_offset); + if(depth>0) + { + medium.m_density = wfi->water_density; + medium.m_pressure = depth*wfi->water_density*wfi->m_gravity.length(); + } + } +} + +// +static inline void ApplyClampedForce( btSoftBody::Node& n, + const btVector3& f, + btScalar dt) +{ + const btScalar dtim=dt*n.m_im; + if((f*dtim).length2()>n.m_v.length2()) + {/* Clamp */ + n.m_f-=ProjectOnAxis(n.m_v,f.normalized())/dtim; + } + else + {/* Apply */ + n.m_f+=f; + } +} + +// +static inline int MatchEdge( const btSoftBody::Node* a, + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) +{ + if((a==ma)&&(b==mb)) return(0); + if((a==mb)&&(b==ma)) return(1); + return(-1); +} + +// +// btEigen : Extract eigen system, +// straitforward implementation of http://math.fullerton.edu/mathews/n2003/JacobiMethodMod.html +// outputs are NOT sorted. +// +struct btEigen +{ + static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + { + static const int maxiterations=16; + static const btScalar accuracy=(btScalar)0.0001; + btMatrix3x3& v=*vectors; + int iterations=0; + vectors->setIdentity(); + do { + int p=0,q=1; + if(btFabs(a[p][q])accuracy) + { + const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); + const btScalar z=btFabs(w); + const btScalar t=w/(z*(btSqrt(1+w*w)+z)); + if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + { + const btScalar c=1/btSqrt(t*t+1); + const btScalar s=c*t; + mulPQ(a,c,s,p,q); + mulTPQ(a,c,s,p,q); + mulPQ(v,c,s,p,q); + } else break; + } else break; + } while((++iterations)accuracy) det=ndet; else break; + } + /* Final orthogonalization */ + Orthogonalize(q); + /* Compute 'S' */ + s=q.transpose()*m; + } + else + { + q.setIdentity(); + s.setIdentity(); + } + return(i); +} + +// +// btSoftColliders +// +struct btSoftColliders +{ + // + // ClusterBase + // + struct ClusterBase : btDbvt::ICollide + { + btScalar erp; + btScalar idt; + btScalar margin; + btScalar friction; + btScalar threshold; + ClusterBase() + { + erp =(btScalar)1; + idt =0; + margin =0; + friction =0; + threshold =(btScalar)0; + } + bool SolveContact( const btGjkEpaSolver2::sResults& res, + btSoftBody::Body ba,btSoftBody::Body bb, + btSoftBody::CJoint& joint) + { + if(res.distancedata; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)m_colObj->getCollisionShape(); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), + rshape,m_colObj->getInterpolationWorldTransform(), + btVector3(1,0,0),res)) + { + btSoftBody::CJoint joint; + if(SolveContact(res,cluster,m_colObj,joint))//prb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;psb->m_joints.push_back(pj); + if(m_colObj->isStaticOrKinematicObject()) + { + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + } + else + { + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + } + } + } + } + void Process(btSoftBody* ps,btCollisionObject* colOb) + { + psb = ps; + m_colObj = colOb; + idt = ps->m_sst.isdt; + margin = m_colObj->getCollisionShape()->getMargin()+ + m_colObj->getCollisionShape()->getMargin(); + friction = btMin(psb->m_cfg.kDF,m_colObj->getFriction()); + btVector3 mins; + btVector3 maxs; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + colOb->getCollisionShape()->getAabb(colOb->getInterpolationWorldTransform(),mins,maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(1,1,1)*margin); + ps->m_cdbvt.collideTV(ps->m_cdbvt.m_root,volume,*this); + } + }; + // + // CollideCL_SS + // + struct CollideCL_SS : ClusterBase + { + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la,const btDbvtNode* lb) + { + btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), + &csb,btTransform::getIdentity(), + cla->m_com-clb->m_com,res)) + { + btSoftBody::CJoint joint; + if(SolveContact(res,cla,clb,joint)) + { + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + } + } + } + void Process(btSoftBody* psa,btSoftBody* psb) + { + idt = psa->m_sst.isdt; + margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); + } + }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m=n.m_im>0?dynmargin:stamargin; + btSoftBody::RContact c; + if( (!n.m_battach)&& + psb->checkContact(m_colObj1,n.m_x,m,c.m_cti)) + { + const btScalar ima=n.m_im; + const btScalar imb= m_rigidBody? m_rigidBody->getInvMass() : 0.f; + const btScalar ms=ima+imb; + if(ms>0) + { + const btTransform& wtr=m_rigidBody?m_rigidBody->getInterpolationWorldTransform() : m_colObj1->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0,0,0,0,0,0,0,0,0); + const btMatrix3x3& iwi=m_rigidBody?m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra)*psb->m_sst.sdt : btVector3(0,0,0); + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=psb->m_cfg.kDF*m_colObj1->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*psb->m_sst.sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + c.m_c4 = m_colObj1->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + btCollisionObject* m_colObj1; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // + // CollideVF_SS + // + struct CollideVF_SS : btDbvt::ICollide + { + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) + { + btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; + btSoftBody::Face* face=(btSoftBody::Face*)lface->data; + btVector3 o=node->m_x; + btVector3 p; + btScalar d=SIMD_INFINITY; + ProjectOrigin( face->m_n[0]->m_x-o, + face->m_n[1]->m_x-o, + face->m_n[2]->m_x-o, + p,d); + const btScalar m=mrg+(o-node->m_q).length()*2; + if(d<(m*m)) + { + const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; + const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); + const btScalar ma=node->m_im; + btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); + if( (n[0]->m_im<=0)|| + (n[1]->m_im<=0)|| + (n[2]->m_im<=0)) + { + mb=0; + } + const btScalar ms=ma+mb; + if(ms>0) + { + btSoftBody::SContact c; + c.m_normal = p/-btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + psb[0]->m_scontacts.push_back(c); + } + } + } + btSoftBody* psb[2]; + btScalar mrg; + }; +}; + +#endif //_BT_SOFT_BODY_INTERNALS_H diff --git a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index eec75bfd3..f5a67f6d8 100644 --- a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -1,134 +1,134 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftBodyRigidBodyCollisionConfiguration.h" -#include "btSoftRigidCollisionAlgorithm.h" -#include "btSoftBodyConcaveCollisionAlgorithm.h" -#include "btSoftSoftCollisionAlgorithm.h" - -#include "LinearMath/btPoolAllocator.h" - -#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 - -btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) -:btDefaultCollisionConfiguration(constructionInfo) -{ - void* mem; - - mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); - m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - m_swappedSoftRigidConvexCreateFunc->m_swapped=true; - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - - mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); - m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; - m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; -#endif - - //replace pool by a new one, with potential larger size - - if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) - { - int curElemSize = m_collisionAlgorithmPool->getElementSize(); - ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - - int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); - int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); - int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); - - int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); - collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); - - if (collisionAlgorithmMaxElementSize > curElemSize) - { - m_collisionAlgorithmPool->~btPoolAllocator(); - btAlignedFree(m_collisionAlgorithmPool); - void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); - m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); - } - } - -} - -btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() -{ - m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softSoftCreateFunc); - - m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConvexCreateFunc); - - m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConvexCreateFunc); - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_softRigidConcaveCreateFunc); - - m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); - btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); -#endif -} - -///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation -btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) -{ - - ///try to handle the softbody interactions first - - if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) - { - return m_softSoftCreateFunc; - } - - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_softRigidConvexCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConvexCreateFunc; - } - -#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS - ///softbody versus convex - if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_softRigidConcaveCreateFunc; - } - - ///convex versus soft body - if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) - { - return m_swappedSoftRigidConcaveCreateFunc; - } -#endif - - ///fallback to the regular rigid collision shape - return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSoftBodyRigidBodyCollisionConfiguration.h" +#include "btSoftRigidCollisionAlgorithm.h" +#include "btSoftBodyConcaveCollisionAlgorithm.h" +#include "btSoftSoftCollisionAlgorithm.h" + +#include "LinearMath/btPoolAllocator.h" + +#define ENABLE_SOFTBODY_CONCAVE_COLLISIONS 1 + +btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo) +:btDefaultCollisionConfiguration(constructionInfo) +{ + void* mem; + + mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); + m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); + m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); + m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; + m_swappedSoftRigidConvexCreateFunc->m_swapped=true; + +#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); + m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; + + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); + m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; + m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; +#endif + + //replace pool by a new one, with potential larger size + + if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) + { + int curElemSize = m_collisionAlgorithmPool->getElementSize(); + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool + + + int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); + int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); + int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); + + int collisionAlgorithmMaxElementSize = btMax(maxSize0,maxSize1); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize2); + + if (collisionAlgorithmMaxElementSize > curElemSize) + { + m_collisionAlgorithmPool->~btPoolAllocator(); + btAlignedFree(m_collisionAlgorithmPool); + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,constructionInfo.m_defaultMaxCollisionAlgorithmPoolSize); + } + } + +} + +btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfiguration() +{ + m_softSoftCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_softSoftCreateFunc); + + m_softRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_softRigidConvexCreateFunc); + + m_swappedSoftRigidConvexCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedSoftRigidConvexCreateFunc); + +#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS + m_softRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_softRigidConcaveCreateFunc); + + m_swappedSoftRigidConcaveCreateFunc->~btCollisionAlgorithmCreateFunc(); + btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); +#endif +} + +///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation +btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) +{ + + ///try to handle the softbody interactions first + + if ((proxyType0 == SOFTBODY_SHAPE_PROXYTYPE ) && (proxyType1==SOFTBODY_SHAPE_PROXYTYPE)) + { + return m_softSoftCreateFunc; + } + + ///softbody versus convex + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_softRigidConvexCreateFunc; + } + + ///convex versus soft body + if (btBroadphaseProxy::isConvex(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + { + return m_swappedSoftRigidConvexCreateFunc; + } + +#ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS + ///softbody versus convex + if (proxyType0 == SOFTBODY_SHAPE_PROXYTYPE && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_softRigidConcaveCreateFunc; + } + + ///convex versus soft body + if (btBroadphaseProxy::isConcave(proxyType0) && proxyType1 == SOFTBODY_SHAPE_PROXYTYPE ) + { + return m_swappedSoftRigidConcaveCreateFunc; + } +#endif + + ///fallback to the regular rigid collision shape + return btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(proxyType0,proxyType1); +} diff --git a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index 17a8296d5..21addcfe2 100644 --- a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -1,48 +1,48 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION -#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - -#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" - -class btVoronoiSimplexSolver; -class btGjkEpaPenetrationDepthSolver; - - -///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration -class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration -{ - - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; - -public: - - btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); - - virtual ~btSoftBodyRigidBodyCollisionConfiguration(); - - ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation - virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); - -}; - -#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION +#define BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION + +#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" + +class btVoronoiSimplexSolver; +class btGjkEpaPenetrationDepthSolver; + + +///btSoftBodyRigidBodyCollisionConfiguration add softbody interaction on top of btDefaultCollisionConfiguration +class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfiguration +{ + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_softSoftCreateFunc; + btCollisionAlgorithmCreateFunc* m_softRigidConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; + +public: + + btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); + + virtual ~btSoftBodyRigidBodyCollisionConfiguration(); + + ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + +}; + +#endif //BT_SOFTBODY_RIGIDBODY_COLLISION_CONFIGURATION + diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index 9a3c9b2e0..b29b35fe6 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -1,79 +1,79 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftRigidCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btSoftBody.h" -///TODO: include all the shapes that the softbody can collide with -///alternatively, implement special case collision algorithms (just like for rigid collision shapes) - -//#include - -btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped) -: btCollisionAlgorithm(ci), -//m_ownManifold(false), -//m_manifoldPtr(mf), -m_isSwapped(isSwapped) -{ -} - - -btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() -{ - - //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); - - /*if (m_ownManifold) - { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); - } - */ - -} - - -#include - -void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)dispatchInfo; - (void)resultOut; - //printf("btSoftRigidCollisionAlgorithm\n"); - - btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0; - btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1; - - softBody->defaultCollisionHandler(rigidCollisionObject); - - -} - -btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - (void)col0; - (void)col1; - - //not yet - return btScalar(1.); -} - - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSoftRigidCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btSoftBody.h" +///TODO: include all the shapes that the softbody can collide with +///alternatively, implement special case collision algorithms (just like for rigid collision shapes) + +//#include + +btSoftRigidCollisionAlgorithm::btSoftRigidCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*col0*/,btCollisionObject* /*col1*/, bool isSwapped) +: btCollisionAlgorithm(ci), +//m_ownManifold(false), +//m_manifoldPtr(mf), +m_isSwapped(isSwapped) +{ +} + + +btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() +{ + + //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); + + /*if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } + */ + +} + + +#include + +void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)dispatchInfo; + (void)resultOut; + //printf("btSoftRigidCollisionAlgorithm\n"); + + btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0; + btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1; + + softBody->defaultCollisionHandler(rigidCollisionObject); + + +} + +btScalar btSoftRigidCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + (void)col0; + (void)col1; + + //not yet + return btScalar(1.); +} + + + diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index e89570af3..adc3844e3 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -1,75 +1,75 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H -#define SOFT_RIGID_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" -class btPersistentManifold; -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" - -#include "LinearMath/btVector3.h" -class btSoftBody; - -/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody -class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm -{ - // bool m_ownManifold; - // btPersistentManifold* m_manifoldPtr; - - btSoftBody* m_softBody; - btCollisionObject* m_rigidCollisionObject; - - ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean - bool m_isSwapped; - -public: - - btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); - - virtual ~btSoftRigidCollisionAlgorithm(); - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - //we don't add any manifolds - } - - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm)); - if (!m_swapped) - { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false); - } else - { - return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true); - } - } - }; - -}; - -#endif //SOFT_RIGID_COLLISION_ALGORITHM_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H +#define SOFT_RIGID_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" + +#include "LinearMath/btVector3.h" +class btSoftBody; + +/// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody +class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm +{ + // bool m_ownManifold; + // btPersistentManifold* m_manifoldPtr; + + btSoftBody* m_softBody; + btCollisionObject* m_rigidCollisionObject; + + ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean + bool m_isSwapped; + +public: + + btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); + + virtual ~btSoftRigidCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + //we don't add any manifolds + } + + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftRigidCollisionAlgorithm)); + if (!m_swapped) + { + return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new(mem) btSoftRigidCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //SOFT_RIGID_COLLISION_ALGORITHM_H + + diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index e8aaad036..1418746b0 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -1,135 +1,135 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btSoftRigidDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//softbody & helpers -#include "btSoftBody.h" -#include "btSoftBodyHelpers.h" - - - - - -btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) -{ - m_drawFlags = fDrawFlags::Std; - m_drawNodeTree = true; - m_drawFaceTree = false; - m_drawClusterTree = false; - m_sbi.m_broadphase = pairCache; - m_sbi.m_dispatcher = dispatcher; - m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.Reset(); - -} - -btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() -{ - -} - -void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep); - - for ( int i=0;ipredictMotion(timeStep); - } -} - -void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) -{ - btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); - - ///solve soft bodies constraints - solveSoftBodiesConstraints(); - - ///update soft bodies - updateSoftBodies(); - -} - -void btSoftRigidDynamicsWorld::updateSoftBodies() -{ - BT_PROFILE("updateSoftBodies"); - - for ( int i=0;iintegrateMotion(); - } -} - -void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() -{ - BT_PROFILE("solveSoftConstraints"); - - if(m_softBodies.size()) - { - btSoftBody::solveClusters(m_softBodies); - } - - for(int i=0;isolveConstraints(); - } -} - -void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body) -{ - m_softBodies.push_back(body); - - btCollisionWorld::addCollisionObject(body, - btBroadphaseProxy::DefaultFilter, - btBroadphaseProxy::AllFilter); - -} - -void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) -{ - m_softBodies.remove(body); - - btCollisionWorld::removeCollisionObject(body); -} - -void btSoftRigidDynamicsWorld::debugDrawWorld() -{ - btDiscreteDynamicsWorld::debugDrawWorld(); - - if (getDebugDrawer()) - { - int i; - for ( i=0;im_softBodies.size();i++) - { - btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); - btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); - if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); - if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); - } - } - } -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btSoftRigidDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//softbody & helpers +#include "btSoftBody.h" +#include "btSoftBodyHelpers.h" + + + + + +btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + +} + +btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() +{ + +} + +void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep); + + for ( int i=0;ipredictMotion(timeStep); + } +} + +void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +{ + btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); + + ///solve soft bodies constraints + solveSoftBodiesConstraints(); + + ///update soft bodies + updateSoftBodies(); + +} + +void btSoftRigidDynamicsWorld::updateSoftBodies() +{ + BT_PROFILE("updateSoftBodies"); + + for ( int i=0;iintegrateMotion(); + } +} + +void btSoftRigidDynamicsWorld::solveSoftBodiesConstraints() +{ + BT_PROFILE("solveSoftConstraints"); + + if(m_softBodies.size()) + { + btSoftBody::solveClusters(m_softBodies); + } + + for(int i=0;isolveConstraints(); + } +} + +void btSoftRigidDynamicsWorld::addSoftBody(btSoftBody* body) +{ + m_softBodies.push_back(body); + + btCollisionWorld::addCollisionObject(body, + btBroadphaseProxy::DefaultFilter, + btBroadphaseProxy::AllFilter); + +} + +void btSoftRigidDynamicsWorld::removeSoftBody(btSoftBody* body) +{ + m_softBodies.remove(body); + + btCollisionWorld::removeCollisionObject(body); +} + +void btSoftRigidDynamicsWorld::debugDrawWorld() +{ + btDiscreteDynamicsWorld::debugDrawWorld(); + + if (getDebugDrawer()) + { + int i; + for ( i=0;im_softBodies.size();i++) + { + btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; + btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); + btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + if(m_drawNodeTree) btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer); + if(m_drawFaceTree) btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer); + if(m_drawClusterTree) btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer); + } + } + } +} diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index c814b7528..edb848e24 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -1,82 +1,82 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H -#define BT_SOFT_RIGID_DYNAMICS_WORLD_H - -#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "btSoftBody.h" - -typedef btAlignedObjectArray btSoftBodyArray; - -class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld -{ - - btSoftBodyArray m_softBodies; - int m_drawFlags; - bool m_drawNodeTree; - bool m_drawFaceTree; - bool m_drawClusterTree; - btSoftBodyWorldInfo m_sbi; - -protected: - - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void updateSoftBodies(); - - void solveSoftBodiesConstraints(); - - -public: - - btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - - virtual ~btSoftRigidDynamicsWorld(); - - virtual void debugDrawWorld(); - - void addSoftBody(btSoftBody* body); - - void removeSoftBody(btSoftBody* body); - - int getDrawFlags() const { return(m_drawFlags); } - void setDrawFlags(int f) { m_drawFlags=f; } - - btSoftBodyWorldInfo& getWorldInfo() - { - return m_sbi; - } - const btSoftBodyWorldInfo& getWorldInfo() const - { - return m_sbi; - } - - - btSoftBodyArray& getSoftBodyArray() - { - return m_softBodies; - } - - const btSoftBodyArray& getSoftBodyArray() const - { - return m_softBodies; - } - -}; - -#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SOFT_RIGID_DYNAMICS_WORLD_H +#define BT_SOFT_RIGID_DYNAMICS_WORLD_H + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "btSoftBody.h" + +typedef btAlignedObjectArray btSoftBodyArray; + +class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld +{ + + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + +protected: + + virtual void predictUnconstraintMotion(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void updateSoftBodies(); + + void solveSoftBodiesConstraints(); + + +public: + + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + + virtual ~btSoftRigidDynamicsWorld(); + + virtual void debugDrawWorld(); + + void addSoftBody(btSoftBody* body); + + void removeSoftBody(btSoftBody* body); + + int getDrawFlags() const { return(m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags=f; } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + +}; + +#endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp index 2ae8c26e2..85a727944 100644 --- a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp @@ -1,46 +1,46 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btSoftSoftCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "btSoftBody.h" - -#define USE_PERSISTENT_CONTACTS 1 - -btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/) -: btCollisionAlgorithm(ci) -//m_ownManifold(false), -//m_manifoldPtr(mf) -{ -} - -btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() -{ -} - -void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - btSoftBody* soft0 = (btSoftBody*)body0; - btSoftBody* soft1 = (btSoftBody*)body1; - soft0->defaultCollisionHandler(soft1); -} - -btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) -{ - //not yet - return 1.f; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btSoftSoftCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "btSoftBody.h" + +#define USE_PERSISTENT_CONTACTS 1 + +btSoftSoftCollisionAlgorithm::btSoftSoftCollisionAlgorithm(btPersistentManifold* /*mf*/,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* /*obj0*/,btCollisionObject* /*obj1*/) +: btCollisionAlgorithm(ci) +//m_ownManifold(false), +//m_manifoldPtr(mf) +{ +} + +btSoftSoftCollisionAlgorithm::~btSoftSoftCollisionAlgorithm() +{ +} + +void btSoftSoftCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + btSoftBody* soft0 = (btSoftBody*)body0; + btSoftBody* soft1 = (btSoftBody*)body1; + soft0->defaultCollisionHandler(soft1); +} + +btScalar btSoftSoftCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) +{ + //not yet + return 1.f; +} diff --git a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 415c4b42a..1b34e0af6 100644 --- a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -1,69 +1,69 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H -#define SOFT_SOFT_COLLISION_ALGORITHM_H - -#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/BroadphaseCollision/btDispatcher.h" -#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" - -class btPersistentManifold; -class btSoftBody; - -///collision detection between two btSoftBody shapes -class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm -{ - bool m_ownManifold; - btPersistentManifold* m_manifoldPtr; - - btSoftBody* m_softBody0; - btSoftBody* m_softBody1; - - -public: - btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) - : btCollisionAlgorithm(ci) {} - - virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); - - virtual void getAllContactManifolds(btManifoldArray& manifoldArray) - { - if (m_manifoldPtr && m_ownManifold) - manifoldArray.push_back(m_manifoldPtr); - } - - btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); - - virtual ~btSoftSoftCollisionAlgorithm(); - - struct CreateFunc :public btCollisionAlgorithmCreateFunc - { - virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) - { - int bbsize = sizeof(btSoftSoftCollisionAlgorithm); - void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); - return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1); - } - }; - -}; - -#endif //SOFT_SOFT_COLLISION_ALGORITHM_H - - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H +#define SOFT_SOFT_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" + +class btPersistentManifold; +class btSoftBody; + +///collision detection between two btSoftBody shapes +class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + + btSoftBody* m_softBody0; + btSoftBody* m_softBody1; + + +public: + btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + if (m_manifoldPtr && m_ownManifold) + manifoldArray.push_back(m_manifoldPtr); + } + + btSoftSoftCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btSoftSoftCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + int bbsize = sizeof(btSoftSoftCollisionAlgorithm); + void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); + return new(ptr) btSoftSoftCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //SOFT_SOFT_COLLISION_ALGORITHM_H + + diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index 955881751..cc4266732 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -1,306 +1,306 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -///btSparseSdf implementation by Nathanael Presson - -#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ -#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ - -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" - -// Modified Paul Hsieh hash -template -unsigned int HsiehHash(const void* pdata) -{ - const unsigned short* data=(const unsigned short*)pdata; - unsigned hash=DWORDLEN<<2,tmp; - for(int i=0;i>11; - } - hash^=hash<<3;hash+=hash>>5; - hash^=hash<<4;hash+=hash>>17; - hash^=hash<<25;hash+=hash>>6; - return(hash); -} - -template -struct btSparseSdf -{ - // - // Inner types - // - struct IntFrac - { - int b; - int i; - btScalar f; - }; - struct Cell - { - btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; - int c[3]; - int puid; - unsigned hash; - btCollisionShape* pclient; - Cell* next; - }; - // - // Fields - // - - btAlignedObjectArray cells; - btScalar voxelsz; - int puid; - int ncells; - int nprobes; - int nqueries; - - // - // Methods - // - - // - void Initialize(int hashsize=2383) - { - cells.resize(hashsize,0); - Reset(); - } - // - void Reset() - { - for(int i=0,ni=cells.size();inext; - delete pc; - pc=pn; - } - } - voxelsz =0.25; - puid =0; - ncells =0; - nprobes =1; - nqueries =1; - } - // - void GarbageCollect(int lifetime=256) - { - const int life=puid-lifetime; - for(int i=0;inext; - if(pc->puidnext=pn; else root=pn; - delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; - } - } - //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); - nqueries=1; - nprobes=1; - ++puid; ///@todo: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ - } - // - int RemoveReferences(btCollisionShape* pcs) - { - int refcount=0; - for(int i=0;inext; - if(pc->pclient==pcs) - { - if(pp) pp->next=pn; else root=pn; - delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; - } - } - return(refcount); - } - // - btScalar Evaluate( const btVector3& x, - btCollisionShape* shape, - btVector3& normal, - btScalar margin) - { - /* Lookup cell */ - const btVector3 scx=x/voxelsz; - const IntFrac ix=Decompose(scx.x()); - const IntFrac iy=Decompose(scx.y()); - const IntFrac iz=Decompose(scx.z()); - const unsigned h=Hash(ix.b,iy.b,iz.b,shape); - Cell*& root=cells[static_cast(h%cells.size())]; - Cell* c=root; - ++nqueries; - while(c) - { - ++nprobes; - if( (c->hash==h) && - (c->c[0]==ix.b) && - (c->c[1]==iy.b) && - (c->c[2]==iz.b) && - (c->pclient==shape)) - { break; } - else - { c=c->next; } - } - if(!c) - { - ++nprobes; - ++ncells; - c=new Cell(); - c->next=root;root=c; - c->pclient=shape; - c->hash=h; - c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; - BuildCell(*c); - } - c->puid=puid; - /* Extract infos */ - const int o[]={ ix.i,iy.i,iz.i}; - const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; - /* Normal */ -#if 1 - const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; - const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; - const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; - normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); - normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); - normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); - normal = normal.normalized(); -#else - normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); -#endif - /* Distance */ - const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); - const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); - return(Lerp(d0,d1,iz.f)-margin); - } - // - void BuildCell(Cell& c) - { - const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; - for(int k=0;k<=CELLSIZE;++k) - { - const btScalar z=voxelsz*k+org.z(); - for(int j=0;j<=CELLSIZE;++j) - { - const btScalar y=voxelsz*j+org.y(); - for(int i=0;i<=CELLSIZE;++i) - { - const btScalar x=voxelsz*i+org.x(); - c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } - } - } - } - // - static inline btScalar DistanceToShape(const btVector3& x, - btCollisionShape* shape) - { - btTransform unit; - unit.setIdentity(); - if(shape->isConvex()) - { - btGjkEpaSolver2::sResults res; - btConvexShape* csh=static_cast(shape); - return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); - } - // - static inline IntFrac Decompose(btScalar x) - { - /* That one need a lot of improvements... */ - /* Remove test, faster floor... */ - IntFrac r; - x/=CELLSIZE; - const int o=x<0?(int)(-x+1):0; - x+=o;r.b=(int)x; - const btScalar k=(x-r.b)*CELLSIZE; - r.i=(int)k;r.f=k-r.i;r.b-=o; - return(r); - } - // - static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { - return(a+(b-a)*t); - } - - - - // - static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) - { - struct btS - { - int x,y,z; - void* p; - }; - - btS myset; - - myset.x=x;myset.y=y;myset.z=z;myset.p=shape; - const void* ptr = &myset; - - unsigned int result = HsiehHash (ptr); - - - return result; - } -}; - - -#endif +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///btSparseSdf implementation by Nathanael Presson + +#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ +#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" + +// Modified Paul Hsieh hash +template +unsigned int HsiehHash(const void* pdata) +{ + const unsigned short* data=(const unsigned short*)pdata; + unsigned hash=DWORDLEN<<2,tmp; + for(int i=0;i>11; + } + hash^=hash<<3;hash+=hash>>5; + hash^=hash<<4;hash+=hash>>17; + hash^=hash<<25;hash+=hash>>6; + return(hash); +} + +template +struct btSparseSdf +{ + // + // Inner types + // + struct IntFrac + { + int b; + int i; + btScalar f; + }; + struct Cell + { + btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; + int c[3]; + int puid; + unsigned hash; + btCollisionShape* pclient; + Cell* next; + }; + // + // Fields + // + + btAlignedObjectArray cells; + btScalar voxelsz; + int puid; + int ncells; + int nprobes; + int nqueries; + + // + // Methods + // + + // + void Initialize(int hashsize=2383) + { + cells.resize(hashsize,0); + Reset(); + } + // + void Reset() + { + for(int i=0,ni=cells.size();inext; + delete pc; + pc=pn; + } + } + voxelsz =0.25; + puid =0; + ncells =0; + nprobes =1; + nqueries =1; + } + // + void GarbageCollect(int lifetime=256) + { + const int life=puid-lifetime; + for(int i=0;inext; + if(pc->puidnext=pn; else root=pn; + delete pc;pc=pp;--ncells; + } + pp=pc;pc=pn; + } + } + //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); + nqueries=1; + nprobes=1; + ++puid; ///@todo: Reset puid's when int range limit is reached */ + /* else setup a priority list... */ + } + // + int RemoveReferences(btCollisionShape* pcs) + { + int refcount=0; + for(int i=0;inext; + if(pc->pclient==pcs) + { + if(pp) pp->next=pn; else root=pn; + delete pc;pc=pp;++refcount; + } + pp=pc;pc=pn; + } + } + return(refcount); + } + // + btScalar Evaluate( const btVector3& x, + btCollisionShape* shape, + btVector3& normal, + btScalar margin) + { + /* Lookup cell */ + const btVector3 scx=x/voxelsz; + const IntFrac ix=Decompose(scx.x()); + const IntFrac iy=Decompose(scx.y()); + const IntFrac iz=Decompose(scx.z()); + const unsigned h=Hash(ix.b,iy.b,iz.b,shape); + Cell*& root=cells[static_cast(h%cells.size())]; + Cell* c=root; + ++nqueries; + while(c) + { + ++nprobes; + if( (c->hash==h) && + (c->c[0]==ix.b) && + (c->c[1]==iy.b) && + (c->c[2]==iz.b) && + (c->pclient==shape)) + { break; } + else + { c=c->next; } + } + if(!c) + { + ++nprobes; + ++ncells; + c=new Cell(); + c->next=root;root=c; + c->pclient=shape; + c->hash=h; + c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; + BuildCell(*c); + } + c->puid=puid; + /* Extract infos */ + const int o[]={ ix.i,iy.i,iz.i}; + const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+1][o[2]+1], + c->d[o[0]+0][o[1]+1][o[2]+1]}; + /* Normal */ +#if 1 + const btScalar gx[]={ d[1]-d[0],d[2]-d[3], + d[5]-d[4],d[6]-d[7]}; + const btScalar gy[]={ d[3]-d[0],d[2]-d[1], + d[7]-d[4],d[6]-d[5]}; + const btScalar gz[]={ d[4]-d[0],d[5]-d[1], + d[7]-d[3],d[6]-d[2]}; + normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), + Lerp(gx[2],gx[3],iy.f),iz.f)); + normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), + Lerp(gy[2],gy[3],ix.f),iz.f)); + normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), + Lerp(gz[2],gz[3],ix.f),iy.f)); + normal = normal.normalized(); +#else + normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); +#endif + /* Distance */ + const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), + Lerp(d[3],d[2],ix.f),iy.f); + const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), + Lerp(d[7],d[6],ix.f),iy.f); + return(Lerp(d0,d1,iz.f)-margin); + } + // + void BuildCell(Cell& c) + { + const btVector3 org=btVector3( (btScalar)c.c[0], + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE*voxelsz; + for(int k=0;k<=CELLSIZE;++k) + { + const btScalar z=voxelsz*k+org.z(); + for(int j=0;j<=CELLSIZE;++j) + { + const btScalar y=voxelsz*j+org.y(); + for(int i=0;i<=CELLSIZE;++i) + { + const btScalar x=voxelsz*i+org.x(); + c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), + c.pclient); + } + } + } + } + // + static inline btScalar DistanceToShape(const btVector3& x, + btCollisionShape* shape) + { + btTransform unit; + unit.setIdentity(); + if(shape->isConvex()) + { + btGjkEpaSolver2::sResults res; + btConvexShape* csh=static_cast(shape); + return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); + } + return(0); + } + // + static inline IntFrac Decompose(btScalar x) + { + /* That one need a lot of improvements... */ + /* Remove test, faster floor... */ + IntFrac r; + x/=CELLSIZE; + const int o=x<0?(int)(-x+1):0; + x+=o;r.b=(int)x; + const btScalar k=(x-r.b)*CELLSIZE; + r.i=(int)k;r.f=k-r.i;r.b-=o; + return(r); + } + // + static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) + { + return(a+(b-a)*t); + } + + + + // + static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) + { + struct btS + { + int x,y,z; + void* p; + }; + + btS myset; + + myset.x=x;myset.y=y;myset.z=z;myset.p=shape; + const void* ptr = &myset; + + unsigned int result = HsiehHash (ptr); + + + return result; + } +}; + + +#endif diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index f7008f22c..99d5a6a2f 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -1,51 +1,51 @@ - -INCLUDE_DIRECTORIES( -${BULLET_PHYSICS_SOURCE_DIR}/src } -) - -SET(LinearMath_SRCS - btConvexHull.cpp - btQuickprof.cpp - btGeometryUtil.cpp - btAlignedAllocator.cpp -) - -SET(LinearMath_HDRS - btAlignedObjectArray.h - btList.h - btPoolAllocator.h - btRandom.h - btVector3.h - btDefaultMotionState.h - btMatrix3x3.h - btQuadWord.h - btHashMap.h - btScalar.h - btAabbUtil2.h - btConvexHull.h - btMinMax.h - btQuaternion.h - btStackAlloc.h - btGeometryUtil.h - btMotionState.h - btTransform.h - btAlignedAllocator.h - btIDebugDraw.h - btQuickprof.h - btTransformUtil.h -) - -ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) -SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) -SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) - -#FILES_MATCHING requires CMake 2.6 -IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - INSTALL(TARGETS LinearMath DESTINATION lib) - INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") -ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) - -IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) - SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) - SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") -ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src } +) + +SET(LinearMath_SRCS + btConvexHull.cpp + btQuickprof.cpp + btGeometryUtil.cpp + btAlignedAllocator.cpp +) + +SET(LinearMath_HDRS + btAlignedObjectArray.h + btList.h + btPoolAllocator.h + btRandom.h + btVector3.h + btDefaultMotionState.h + btMatrix3x3.h + btQuadWord.h + btHashMap.h + btScalar.h + btAabbUtil2.h + btConvexHull.h + btMinMax.h + btQuaternion.h + btStackAlloc.h + btGeometryUtil.h + btMotionState.h + btTransform.h + btAlignedAllocator.h + btIDebugDraw.h + btQuickprof.h + btTransformUtil.h +) + +ADD_LIBRARY(LinearMath ${LinearMath_SRCS} ${LinearMath_HDRS}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES VERSION ${BULLET_VERSION}) +SET_TARGET_PROPERTIES(LinearMath PROPERTIES SOVERSION ${BULLET_VERSION}) + +#FILES_MATCHING requires CMake 2.6 +IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + INSTALL(TARGETS LinearMath DESTINATION lib) + INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION include FILES_MATCHING PATTERN "*.h") +ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5) + +IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES FRAMEWORK true) + SET_TARGET_PROPERTIES(LinearMath PROPERTIES PUBLIC_HEADER "${LinearMath_HDRS}") +ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK) diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp index f9e8b01cc..a3d790f8a 100644 --- a/src/LinearMath/btAlignedAllocator.cpp +++ b/src/LinearMath/btAlignedAllocator.cpp @@ -1,205 +1,205 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "btAlignedAllocator.h" - -int gNumAlignedAllocs = 0; -int gNumAlignedFree = 0; -int gTotalBytesAlignedAllocs = 0;//detect memory leaks - -static void *btAllocDefault(size_t size) -{ - return malloc(size); -} - -static void btFreeDefault(void *ptr) -{ - free(ptr); -} - -static btAllocFunc *sAllocFunc = btAllocDefault; -static btFreeFunc *sFreeFunc = btFreeDefault; - - - -#if defined (BT_HAS_ALIGNED_ALLOCATOR) -#include -static void *btAlignedAllocDefault(size_t size, int alignment) -{ - return _aligned_malloc(size, (size_t)alignment); -} - -static void btAlignedFreeDefault(void *ptr) -{ - _aligned_free(ptr); -} -#elif defined(__CELLOS_LV2__) -#include - -static inline void *btAlignedAllocDefault(size_t size, int alignment) -{ - return memalign(alignment, size); -} - -static inline void btAlignedFreeDefault(void *ptr) -{ - free(ptr); -} -#else -static inline void *btAlignedAllocDefault(size_t size, int alignment) -{ - void *ret; - char *real; - unsigned long offset; - - real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); - if (real) { - offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); - ret = (void *)((real + sizeof(void *)) + offset); - *((void **)(ret)-1) = (void *)(real); - } else { - ret = (void *)(real); - } - return (ret); -} - -static inline void btAlignedFreeDefault(void *ptr) -{ - void* real; - - if (ptr) { - real = *((void **)(ptr)-1); - sFreeFunc(real); - } -} -#endif - - -static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; -static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; - -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) -{ - sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; - sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; -} - -void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) -{ - sAllocFunc = allocFunc ? allocFunc : btAllocDefault; - sFreeFunc = freeFunc ? freeFunc : btFreeDefault; -} - -#ifdef BT_DEBUG_MEMORY_ALLOCATIONS -//this generic allocator provides the total allocated number of bytes -#include - -void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename) -{ - void *ret; - char *real; - unsigned long offset; - - gTotalBytesAlignedAllocs += size; - gNumAlignedAllocs++; - - - real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); - if (real) { - offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) & -(alignment-1); - ret = (void *)((real + 2*sizeof(void *)) + offset); - *((void **)(ret)-1) = (void *)(real); - *((int*)(ret)-2) = size; - - } else { - ret = (void *)(real);//?? - } - - printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); - - int* ptr = (int*)ret; - *ptr = 12; - return (ret); -} - -void btAlignedFreeInternal (void* ptr,int line,char* filename) -{ - - void* real; - gNumAlignedFree++; - - if (ptr) { - real = *((void **)(ptr)-1); - int size = *((int*)(ptr)-2); - gTotalBytesAlignedAllocs -= size; - - printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size); - - sFreeFunc(real); - } else - { - printf("NULL ptr\n"); - } -} - -#else //BT_DEBUG_MEMORY_ALLOCATIONS - -void* btAlignedAllocInternal (size_t size, int alignment) -{ - gNumAlignedAllocs++; - void* ptr; -#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) - ptr = sAlignedAllocFunc(size, alignment); -#else - char *real; - unsigned long offset; - - real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); - if (real) { - offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); - ptr = (void *)((real + sizeof(void *)) + offset); - *((void **)(ptr)-1) = (void *)(real); - } else { - ptr = (void *)(real); - } -#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) -// printf("btAlignedAllocInternal %d, %x\n",size,ptr); - return ptr; -} - -void btAlignedFreeInternal (void* ptr) -{ - if (!ptr) - { - return; - } - - gNumAlignedFree++; -// printf("btAlignedFreeInternal %x\n",ptr); -#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) - sAlignedFreeFunc(ptr); -#else - void* real; - - if (ptr) { - real = *((void **)(ptr)-1); - sFreeFunc(real); - } -#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) -} - -#endif //BT_DEBUG_MEMORY_ALLOCATIONS - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btAlignedAllocator.h" + +int gNumAlignedAllocs = 0; +int gNumAlignedFree = 0; +int gTotalBytesAlignedAllocs = 0;//detect memory leaks + +static void *btAllocDefault(size_t size) +{ + return malloc(size); +} + +static void btFreeDefault(void *ptr) +{ + free(ptr); +} + +static btAllocFunc *sAllocFunc = btAllocDefault; +static btFreeFunc *sFreeFunc = btFreeDefault; + + + +#if defined (BT_HAS_ALIGNED_ALLOCATOR) +#include +static void *btAlignedAllocDefault(size_t size, int alignment) +{ + return _aligned_malloc(size, (size_t)alignment); +} + +static void btAlignedFreeDefault(void *ptr) +{ + _aligned_free(ptr); +} +#elif defined(__CELLOS_LV2__) +#include + +static inline void *btAlignedAllocDefault(size_t size, int alignment) +{ + return memalign(alignment, size); +} + +static inline void btAlignedFreeDefault(void *ptr) +{ + free(ptr); +} +#else +static inline void *btAlignedAllocDefault(size_t size, int alignment) +{ + void *ret; + char *real; + unsigned long offset; + + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); + ret = (void *)((real + sizeof(void *)) + offset); + *((void **)(ret)-1) = (void *)(real); + } else { + ret = (void *)(real); + } + return (ret); +} + +static inline void btAlignedFreeDefault(void *ptr) +{ + void* real; + + if (ptr) { + real = *((void **)(ptr)-1); + sFreeFunc(real); + } +} +#endif + + +static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault; +static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault; + +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc) +{ + sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault; + sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault; +} + +void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc) +{ + sAllocFunc = allocFunc ? allocFunc : btAllocDefault; + sFreeFunc = freeFunc ? freeFunc : btFreeDefault; +} + +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS +//this generic allocator provides the total allocated number of bytes +#include + +void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename) +{ + void *ret; + char *real; + unsigned long offset; + + gTotalBytesAlignedAllocs += size; + gNumAlignedAllocs++; + + + real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) & +(alignment-1); + ret = (void *)((real + 2*sizeof(void *)) + offset); + *((void **)(ret)-1) = (void *)(real); + *((int*)(ret)-2) = size; + + } else { + ret = (void *)(real);//?? + } + + printf("allocation#%d at address %x, from %s,line %d, size %d\n",gNumAlignedAllocs,real, filename,line,size); + + int* ptr = (int*)ret; + *ptr = 12; + return (ret); +} + +void btAlignedFreeInternal (void* ptr,int line,char* filename) +{ + + void* real; + gNumAlignedFree++; + + if (ptr) { + real = *((void **)(ptr)-1); + int size = *((int*)(ptr)-2); + gTotalBytesAlignedAllocs -= size; + + printf("free #%d at address %x, from %s,line %d, size %d\n",gNumAlignedFree,real, filename,line,size); + + sFreeFunc(real); + } else + { + printf("NULL ptr\n"); + } +} + +#else //BT_DEBUG_MEMORY_ALLOCATIONS + +void* btAlignedAllocInternal (size_t size, int alignment) +{ + gNumAlignedAllocs++; + void* ptr; +#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) + ptr = sAlignedAllocFunc(size, alignment); +#else + char *real; + unsigned long offset; + + real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); + if (real) { + offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); + ptr = (void *)((real + sizeof(void *)) + offset); + *((void **)(ptr)-1) = (void *)(real); + } else { + ptr = (void *)(real); + } +#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) +// printf("btAlignedAllocInternal %d, %x\n",size,ptr); + return ptr; +} + +void btAlignedFreeInternal (void* ptr) +{ + if (!ptr) + { + return; + } + + gNumAlignedFree++; +// printf("btAlignedFreeInternal %x\n",ptr); +#if defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) + sAlignedFreeFunc(ptr); +#else + void* real; + + if (ptr) { + real = *((void **)(ptr)-1); + sFreeFunc(real); + } +#endif // defined (BT_HAS_ALIGNED_ALLOCATOR) || defined(__CELLOS_LV2__) +} + +#endif //BT_DEBUG_MEMORY_ALLOCATIONS + diff --git a/src/LinearMath/btAlignedAllocator.h b/src/LinearMath/btAlignedAllocator.h index a95f9911f..f168f3c66 100644 --- a/src/LinearMath/btAlignedAllocator.h +++ b/src/LinearMath/btAlignedAllocator.h @@ -1,107 +1,107 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_ALIGNED_ALLOCATOR -#define BT_ALIGNED_ALLOCATOR - -///we probably replace this with our own aligned memory allocator -///so we replace _aligned_malloc and _aligned_free with our own -///that is better portable and more predictable - -#include "btScalar.h" -//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 -#ifdef BT_DEBUG_MEMORY_ALLOCATIONS - -#define btAlignedAlloc(a,b) \ - btAlignedAllocInternal(a,b,__LINE__,__FILE__) - -#define btAlignedFree(ptr) \ - btAlignedFreeInternal(ptr,__LINE__,__FILE__) - -void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename); - -void btAlignedFreeInternal (void* ptr,int line,char* filename); - -#else - void* btAlignedAllocInternal (size_t size, int alignment); - void btAlignedFreeInternal (void* ptr); - - #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment) - #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) - -#endif -typedef int size_type; - -typedef void *(btAlignedAllocFunc)(size_t size, int alignment); -typedef void (btAlignedFreeFunc)(void *memblock); -typedef void *(btAllocFunc)(size_t size); -typedef void (btFreeFunc)(void *memblock); - -///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom -void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc); -///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. -void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); - - -///The btAlignedAllocator is a portable class for aligned memory allocations. -///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned. -template < typename T , unsigned Alignment > -class btAlignedAllocator { - - typedef btAlignedAllocator< T , Alignment > self_type; - -public: - - //just going down a list: - btAlignedAllocator() {} - /* - btAlignedAllocator( const self_type & ) {} - */ - - template < typename Other > - btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} - - typedef const T* const_pointer; - typedef const T& const_reference; - typedef T* pointer; - typedef T& reference; - typedef T value_type; - - pointer address ( reference ref ) const { return &ref; } - const_pointer address ( const_reference ref ) const { return &ref; } - pointer allocate ( size_type n , const_pointer * hint = 0 ) { - (void)hint; - return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment )); - } - void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } - void deallocate( pointer ptr ) { - btAlignedFree( reinterpret_cast< void * >( ptr ) ); - } - void destroy ( pointer ptr ) { ptr->~value_type(); } - - - template < typename O > struct rebind { - typedef btAlignedAllocator< O , Alignment > other; - }; - template < typename O > - self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; } - - friend bool operator==( const self_type & , const self_type & ) { return true; } -}; - - - -#endif //BT_ALIGNED_ALLOCATOR - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_ALIGNED_ALLOCATOR +#define BT_ALIGNED_ALLOCATOR + +///we probably replace this with our own aligned memory allocator +///so we replace _aligned_malloc and _aligned_free with our own +///that is better portable and more predictable + +#include "btScalar.h" +//#define BT_DEBUG_MEMORY_ALLOCATIONS 1 +#ifdef BT_DEBUG_MEMORY_ALLOCATIONS + +#define btAlignedAlloc(a,b) \ + btAlignedAllocInternal(a,b,__LINE__,__FILE__) + +#define btAlignedFree(ptr) \ + btAlignedFreeInternal(ptr,__LINE__,__FILE__) + +void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename); + +void btAlignedFreeInternal (void* ptr,int line,char* filename); + +#else + void* btAlignedAllocInternal (size_t size, int alignment); + void btAlignedFreeInternal (void* ptr); + + #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment) + #define btAlignedFree(ptr) btAlignedFreeInternal(ptr) + +#endif +typedef int size_type; + +typedef void *(btAlignedAllocFunc)(size_t size, int alignment); +typedef void (btAlignedFreeFunc)(void *memblock); +typedef void *(btAllocFunc)(size_t size); +typedef void (btFreeFunc)(void *memblock); + +///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom +void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc); +///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it. +void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc); + + +///The btAlignedAllocator is a portable class for aligned memory allocations. +///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned. +template < typename T , unsigned Alignment > +class btAlignedAllocator { + + typedef btAlignedAllocator< T , Alignment > self_type; + +public: + + //just going down a list: + btAlignedAllocator() {} + /* + btAlignedAllocator( const self_type & ) {} + */ + + template < typename Other > + btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {} + + typedef const T* const_pointer; + typedef const T& const_reference; + typedef T* pointer; + typedef T& reference; + typedef T value_type; + + pointer address ( reference ref ) const { return &ref; } + const_pointer address ( const_reference ref ) const { return &ref; } + pointer allocate ( size_type n , const_pointer * hint = 0 ) { + (void)hint; + return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment )); + } + void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); } + void deallocate( pointer ptr ) { + btAlignedFree( reinterpret_cast< void * >( ptr ) ); + } + void destroy ( pointer ptr ) { ptr->~value_type(); } + + + template < typename O > struct rebind { + typedef btAlignedAllocator< O , Alignment > other; + }; + template < typename O > + self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; } + + friend bool operator==( const self_type & , const self_type & ) { return true; } +}; + + + +#endif //BT_ALIGNED_ALLOCATOR + diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h index e3d3e30c3..bad1eee1f 100644 --- a/src/LinearMath/btAlignedObjectArray.h +++ b/src/LinearMath/btAlignedObjectArray.h @@ -1,442 +1,442 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_OBJECT_ARRAY__ -#define BT_OBJECT_ARRAY__ - -#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE -#include "btAlignedAllocator.h" - -///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW -///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors -///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator= -///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and -///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240 - -#define BT_USE_PLACEMENT_NEW 1 -//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in or or otherwise... - -#ifdef BT_USE_MEMCPY -#include -#include -#endif //BT_USE_MEMCPY - -#ifdef BT_USE_PLACEMENT_NEW -#include //for placement new -#endif //BT_USE_PLACEMENT_NEW - - -///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods -///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data -template -//template -class btAlignedObjectArray -{ - btAlignedAllocator m_allocator; - - int m_size; - int m_capacity; - T* m_data; - //PCK: added this line - bool m_ownsMemory; - - protected: - SIMD_FORCE_INLINE int allocSize(int size) - { - return (size ? size*2 : 1); - } - SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const - { - int i; - for (i=start;i size()) - { - reserve(newsize); - } -#ifdef BT_USE_PLACEMENT_NEW - for (int i=curSize;i - void quickSortInternal(L CompareFunc,int lo, int hi) - { - // lo is the lower index, hi is the upper index - // of the region of array a that is to be sorted - int i=lo, j=hi; - T x=m_data[(lo+hi)/2]; - - // partition - do - { - while (CompareFunc(m_data[i],x)) - i++; - while (CompareFunc(x,m_data[j])) - j--; - if (i<=j) - { - swap(i,j); - i++; j--; - } - } while (i<=j); - - // recursion - if (lo - void quickSort(L CompareFunc) - { - //don't sort 0 or 1 elements - if (size()>1) - { - quickSortInternal(CompareFunc,0,size()-1); - } - } - - - ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ - template - void downHeap(T *pArr, int k, int n,L CompareFunc) - { - /* PRE: a[k+1..N] is a heap */ - /* POST: a[k..N] is a heap */ - - T temp = pArr[k - 1]; - /* k has child(s) */ - while (k <= n/2) - { - int child = 2*k; - - if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) - { - child++; - } - /* pick larger child */ - if (CompareFunc(temp , pArr[child - 1])) - { - /* move child up */ - pArr[k - 1] = pArr[child - 1]; - k = child; - } - else - { - break; - } - } - pArr[k - 1] = temp; - } /*downHeap*/ - - void swap(int index0,int index1) - { -#ifdef BT_USE_MEMCPY - char temp[sizeof(T)]; - memcpy(temp,&m_data[index0],sizeof(T)); - memcpy(&m_data[index0],&m_data[index1],sizeof(T)); - memcpy(&m_data[index1],temp,sizeof(T)); -#else - T temp = m_data[index0]; - m_data[index0] = m_data[index1]; - m_data[index1] = temp; -#endif //BT_USE_PLACEMENT_NEW - - } - - template - void heapSort(L CompareFunc) - { - /* sort a[0..N-1], N.B. 0 to N-1 */ - int k; - int n = m_size; - for (k = n/2; k > 0; k--) - { - downHeap(m_data, k, n, CompareFunc); - } - - /* a[1..N] is now a heap */ - while ( n>=1 ) - { - swap(0,n-1); /* largest of a[0..n-1] */ - - - n = n - 1; - /* restore a[1..i-1] heap */ - downHeap(m_data, 1, n, CompareFunc); - } - } - - ///non-recursive binary search, assumes sorted array - int findBinarySearch(const T& key) const - { - int first = 0; - int last = size(); - - //assume sorted array - while (first <= last) { - int mid = (first + last) / 2; // compute mid point. - if (key > m_data[mid]) - first = mid + 1; // repeat search in top half. - else if (key < m_data[mid]) - last = mid - 1; // repeat search in bottom half. - else - return mid; // found it. return position ///// - } - return size(); // failed to find key - } - - - int findLinearSearch(const T& key) const - { - int index=size(); - int i; - - for (i=0;i or or otherwise... + +#ifdef BT_USE_MEMCPY +#include +#include +#endif //BT_USE_MEMCPY + +#ifdef BT_USE_PLACEMENT_NEW +#include //for placement new +#endif //BT_USE_PLACEMENT_NEW + + +///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods +///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data +template +//template +class btAlignedObjectArray +{ + btAlignedAllocator m_allocator; + + int m_size; + int m_capacity; + T* m_data; + //PCK: added this line + bool m_ownsMemory; + + protected: + SIMD_FORCE_INLINE int allocSize(int size) + { + return (size ? size*2 : 1); + } + SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const + { + int i; + for (i=start;i size()) + { + reserve(newsize); + } +#ifdef BT_USE_PLACEMENT_NEW + for (int i=curSize;i + void quickSortInternal(L CompareFunc,int lo, int hi) + { + // lo is the lower index, hi is the upper index + // of the region of array a that is to be sorted + int i=lo, j=hi; + T x=m_data[(lo+hi)/2]; + + // partition + do + { + while (CompareFunc(m_data[i],x)) + i++; + while (CompareFunc(x,m_data[j])) + j--; + if (i<=j) + { + swap(i,j); + i++; j--; + } + } while (i<=j); + + // recursion + if (lo + void quickSort(L CompareFunc) + { + //don't sort 0 or 1 elements + if (size()>1) + { + quickSortInternal(CompareFunc,0,size()-1); + } + } + + + ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ + template + void downHeap(T *pArr, int k, int n,L CompareFunc) + { + /* PRE: a[k+1..N] is a heap */ + /* POST: a[k..N] is a heap */ + + T temp = pArr[k - 1]; + /* k has child(s) */ + while (k <= n/2) + { + int child = 2*k; + + if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child])) + { + child++; + } + /* pick larger child */ + if (CompareFunc(temp , pArr[child - 1])) + { + /* move child up */ + pArr[k - 1] = pArr[child - 1]; + k = child; + } + else + { + break; + } + } + pArr[k - 1] = temp; + } /*downHeap*/ + + void swap(int index0,int index1) + { +#ifdef BT_USE_MEMCPY + char temp[sizeof(T)]; + memcpy(temp,&m_data[index0],sizeof(T)); + memcpy(&m_data[index0],&m_data[index1],sizeof(T)); + memcpy(&m_data[index1],temp,sizeof(T)); +#else + T temp = m_data[index0]; + m_data[index0] = m_data[index1]; + m_data[index1] = temp; +#endif //BT_USE_PLACEMENT_NEW + + } + + template + void heapSort(L CompareFunc) + { + /* sort a[0..N-1], N.B. 0 to N-1 */ + int k; + int n = m_size; + for (k = n/2; k > 0; k--) + { + downHeap(m_data, k, n, CompareFunc); + } + + /* a[1..N] is now a heap */ + while ( n>=1 ) + { + swap(0,n-1); /* largest of a[0..n-1] */ + + + n = n - 1; + /* restore a[1..i-1] heap */ + downHeap(m_data, 1, n, CompareFunc); + } + } + + ///non-recursive binary search, assumes sorted array + int findBinarySearch(const T& key) const + { + int first = 0; + int last = size(); + + //assume sorted array + while (first <= last) { + int mid = (first + last) / 2; // compute mid point. + if (key > m_data[mid]) + first = mid + 1; // repeat search in top half. + else if (key < m_data[mid]) + last = mid - 1; // repeat search in bottom half. + else + return mid; // found it. return position ///// + } + return size(); // failed to find key + } + + + int findLinearSearch(const T& key) const + { + int index=size(); + int i; + + for (i=0;i - -#include "btConvexHull.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btMinMax.h" -#include "LinearMath/btVector3.h" - - - -template -void Swap(T &a,T &b) -{ - T tmp = a; - a=b; - b=tmp; -} - - -//---------------------------------- - -class int3 -{ -public: - int x,y,z; - int3(){}; - int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} - const int& operator[](int i) const {return (&x)[i];} - int& operator[](int i) {return (&x)[i];} -}; - - -//------- btPlane ---------- - - -inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} -inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } -inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } - - -//--------- Utility Functions ------ - -btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); -btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); - -btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); -btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) -{ - btVector3 N1 = p0.normal; - btVector3 N2 = p1.normal; - btVector3 N3 = p2.normal; - - btVector3 n2n3; n2n3 = N2.cross(N3); - btVector3 n3n1; n3n1 = N3.cross(N1); - btVector3 n1n2; n1n2 = N1.cross(N2); - - btScalar quotient = (N1.dot(n2n3)); - - btAssert(btFabs(quotient) > btScalar(0.000001)); - - quotient = btScalar(-1.) / quotient; - n2n3 *= p0.dist; - n3n1 *= p1.dist; - n1n2 *= p2.dist; - btVector3 potentialVertex = n2n3; - potentialVertex += n3n1; - potentialVertex += n1n2; - potentialVertex *= quotient; - - btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); - return result; - -} - -btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); -btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); -btVector3 NormalOf(const btVector3 *vert, const int n); - - -btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) -{ - // returns the point where the line p0-p1 intersects the plane n&d - static btVector3 dif; - dif = p1-p0; - btScalar dn= dot(plane.normal,dif); - btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn; - return p0 + (dif*t); -} - -btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) -{ - return point - plane.normal * (dot(point,plane.normal)+plane.dist); -} - -btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) -{ - // return the normal of the triangle - // inscribed by v0, v1, and v2 - btVector3 cp=cross(v1-v0,v2-v1); - btScalar m=cp.length(); - if(m==0) return btVector3(1,0,0); - return cp*(btScalar(1.0)/m); -} - - -btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) -{ - static btVector3 cp; - cp = cross(udir,vdir).normalized(); - - btScalar distu = -dot(cp,ustart); - btScalar distv = -dot(cp,vstart); - btScalar dist = (btScalar)fabs(distu-distv); - if(upoint) - { - btPlane plane; - plane.normal = cross(vdir,cp).normalized(); - plane.dist = -dot(plane.normal,vstart); - *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); - } - if(vpoint) - { - btPlane plane; - plane.normal = cross(udir,cp).normalized(); - plane.dist = -dot(plane.normal,ustart); - *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); - } - return dist; -} - - - - - - - -#define COPLANAR (0) -#define UNDER (1) -#define OVER (2) -#define SPLIT (OVER|UNDER) -#define PAPERWIDTH (btScalar(0.001)) - -btScalar planetestepsilon = PAPERWIDTH; - - - -typedef ConvexH::HalfEdge HalfEdge; - -ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) -{ - vertices.resize(vertices_size); - edges.resize(edges_size); - facets.resize(facets_size); -} - - -int PlaneTest(const btPlane &p, const btVector3 &v); -int PlaneTest(const btPlane &p, const btVector3 &v) { - btScalar a = dot(v,p.normal)+p.dist; - int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); - return flag; -} - -int SplitTest(ConvexH &convex,const btPlane &plane); -int SplitTest(ConvexH &convex,const btPlane &plane) { - int flag=0; - for(int i=0;i -int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray &allow) -{ - btAssert(count); - int m=-1; - for(int i=0;idot(p[m],dir)) - m=i; - } - btAssert(m!=-1); - return m; -} - -btVector3 orth(const btVector3 &v); -btVector3 orth(const btVector3 &v) -{ - btVector3 a=cross(v,btVector3(0,0,1)); - btVector3 b=cross(v,btVector3(0,1,0)); - if (a.length() > b.length()) - { - return a.normalized(); - } else { - return b.normalized(); - } -} - - -template -int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &allow) -{ - int m=-1; - while(m==-1) - { - m = maxdirfiltered(p,count,dir,allow); - if(allow[m]==3) return m; - T u = orth(dir); - T v = cross(u,dir); - int ma=-1; - for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) - { - btScalar s = sinf(SIMD_RADS_PER_DEG*(x)); - btScalar c = cosf(SIMD_RADS_PER_DEG*(x)); - int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(ma==m && mb==m) - { - allow[m]=3; - return m; - } - if(ma!=-1 && ma!=mb) // Yuck - this is really ugly - { - int mc = ma; - for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) - { - btScalar s = sinf(SIMD_RADS_PER_DEG*(xx)); - btScalar c = cosf(SIMD_RADS_PER_DEG*(xx)); - int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); - if(mc==m && md==m) - { - allow[m]=3; - return m; - } - mc=md; - } - } - ma=mb; - } - allow[m]=0; - m=-1; - } - btAssert(0); - return m; -} - - - - -int operator ==(const int3 &a,const int3 &b); -int operator ==(const int3 &a,const int3 &b) -{ - for(int i=0;i<3;i++) - { - if(a[i]!=b[i]) return 0; - } - return 1; -} - - -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); -int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) -{ - btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); - return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? -} -int hasedge(const int3 &t, int a,int b); -int hasedge(const int3 &t, int a,int b) -{ - for(int i=0;i<3;i++) - { - int i1= (i+1)%3; - if(t[i]==a && t[i1]==b) return 1; - } - return 0; -} -int hasvert(const int3 &t, int v); -int hasvert(const int3 &t, int v) -{ - return (t[0]==v || t[1]==v || t[2]==v) ; -} -int shareedge(const int3 &a,const int3 &b); -int shareedge(const int3 &a,const int3 &b) -{ - int i; - for(i=0;i<3;i++) - { - int i1= (i+1)%3; - if(hasedge(a,b[i1],b[i])) return 1; - } - return 0; -} - -class btHullTriangle; - - - -class btHullTriangle : public int3 -{ -public: - int3 n; - int id; - int vmax; - btScalar rise; - btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) - { - vmax=-1; - rise = btScalar(0.0); - } - ~btHullTriangle() - { - } - int &neib(int a,int b); -}; - - -int &btHullTriangle::neib(int a,int b) -{ - static int er=-1; - int i; - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - if((*this)[i]==a && (*this)[i1]==b) return n[i2]; - if((*this)[i]==b && (*this)[i1]==a) return n[i2]; - } - btAssert(0); - return er; -} -void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) -{ - int i; - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - int a = (*s)[i1]; - int b = (*s)[i2]; - btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); - btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); - m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); - m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); - } -} - -void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) -{ - b2bfix(s,t); - deAllocateTriangle(s); - - deAllocateTriangle(t); -} - -void HullLibrary::checkit(btHullTriangle *t) -{ - (void)t; - - int i; - btAssert(m_tris[t->id]==t); - for(i=0;i<3;i++) - { - int i1=(i+1)%3; - int i2=(i+2)%3; - int a = (*t)[i1]; - int b = (*t)[i2]; - - // release compile fix - (void)i1; - (void)i2; - (void)a; - (void)b; - - btAssert(a!=b); - btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); - } -} - -btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) -{ - void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); - btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); - tr->id = m_tris.size(); - m_tris.push_back(tr); - - return tr; -} - -void HullLibrary::deAllocateTriangle(btHullTriangle* tri) -{ - btAssert(m_tris[tri->id]==tri); - m_tris[tri->id]=NULL; - tri->~btHullTriangle(); - btAlignedFree(tri); -} - - -void HullLibrary::extrude(btHullTriangle *t0,int v) -{ - int3 t= *t0; - int n = m_tris.size(); - btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); - ta->n = int3(t0->n[0],n+1,n+2); - m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); - tb->n = int3(t0->n[1],n+2,n+0); - m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); - tc->n = int3(t0->n[2],n+0,n+1); - m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; - checkit(ta); - checkit(tb); - checkit(tc); - if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); - if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); - if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); - deAllocateTriangle(t0); - -} - -btHullTriangle* HullLibrary::extrudable(btScalar epsilon) -{ - int i; - btHullTriangle *t=NULL; - for(i=0;iriserise)) - { - t = m_tris[i]; - } - } - return (t->rise >epsilon)?t:NULL ; -} - - - - -int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) -{ - btVector3 basis[3]; - basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); - int p0 = maxdirsterid(verts,verts_count, basis[0],allow); - int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); - basis[0] = verts[p0]-verts[p1]; - if(p0==p1 || basis[0]==btVector3(0,0,0)) - return int4(-1,-1,-1,-1); - basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); - basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); - if (basis[1].length() > basis[2].length()) - { - basis[1].normalize(); - } else { - basis[1] = basis[2]; - basis[1].normalize (); - } - int p2 = maxdirsterid(verts,verts_count,basis[1],allow); - if(p2 == p0 || p2 == p1) - { - p2 = maxdirsterid(verts,verts_count,-basis[1],allow); - } - if(p2 == p0 || p2 == p1) - return int4(-1,-1,-1,-1); - basis[1] = verts[p2] - verts[p0]; - basis[2] = cross(basis[1],basis[0]).normalized(); - int p3 = maxdirsterid(verts,verts_count,basis[2],allow); - if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); - if(p3==p0||p3==p1||p3==p2) - return int4(-1,-1,-1,-1); - btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); - if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} - return int4(p0,p1,p2,p3); -} - -int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) -{ - if(verts_count <4) return 0; - if(vlimit==0) vlimit=1000000000; - int j; - btVector3 bmin(*verts),bmax(*verts); - btAlignedObjectArray isextreme; - isextreme.reserve(verts_count); - btAlignedObjectArray allow; - allow.reserve(verts_count); - - for(j=0;jn=int3(2,3,1); - btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); - btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); - btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); - isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; - checkit(t0);checkit(t1);checkit(t2);checkit(t3); - - for(j=0;jvmax<0); - btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); - t->vmax = maxdirsterid(verts,verts_count,n,allow); - t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); - } - btHullTriangle *te; - vlimit-=4; - while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) - { - int3 ti=*te; - int v=te->vmax; - btAssert(v != -1); - btAssert(!isextreme[v]); // wtf we've already done this vertex - isextreme[v]=1; - //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already - j=m_tris.size(); - while(j--) { - if(!m_tris[j]) continue; - int3 t=*m_tris[j]; - if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) - { - extrude(m_tris[j],v); - } - } - // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle - j=m_tris.size(); - while(j--) - { - if(!m_tris[j]) continue; - if(!hasvert(*m_tris[j],v)) break; - int3 nt=*m_tris[j]; - if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) - { - btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; - btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; - btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); - t->vmax = maxdirsterid(verts,verts_count,n,allow); - if(isextreme[t->vmax]) - { - t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. - } - else - { - t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); - } - } - vlimit --; - } - return 1; -} - -int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) -{ - int rc=calchullgen(verts,verts_count, vlimit) ; - if(!rc) return 0; - btAlignedObjectArray ts; - int i; - - for(i=0;i(ts[i]); - } - m_tris.resize(0); - - return 1; -} - - - - - -bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) -{ - - int tris_count; - int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast(vlimit) ); - if(!ret) return false; - result.mIndexCount = (unsigned int) (tris_count*3); - result.mFaceCount = (unsigned int) tris_count; - result.mVertices = (btVector3*) vertices; - result.mVcount = (unsigned int) vcount; - return true; - -} - - -void ReleaseHull(PHullResult &result); -void ReleaseHull(PHullResult &result) -{ - if ( result.m_Indices.size() ) - { - result.m_Indices.clear(); - } - - result.mVcount = 0; - result.mIndexCount = 0; - result.mVertices = 0; -} - - -//********************************************************************* -//********************************************************************* -//******** HullLib header -//********************************************************************* -//********************************************************************* - -//********************************************************************* -//********************************************************************* -//******** HullLib implementation -//********************************************************************* -//********************************************************************* - -HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result) // contains the resulst -{ - HullError ret = QE_FAIL; - - - PHullResult hr; - - unsigned int vcount = desc.mVcount; - if ( vcount < 8 ) vcount = 8; - - btAlignedObjectArray vertexSource; - vertexSource.resize(static_cast(vcount)); - - btVector3 scale; - - unsigned int ovcount; - - bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! - - if ( ok ) - { - - -// if ( 1 ) // scale vertices back to their original size. - { - for (unsigned int i=0; i(i)]; - v[0]*=scale[0]; - v[1]*=scale[1]; - v[2]*=scale[2]; - } - } - - ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); - - if ( ok ) - { - - // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. - btAlignedObjectArray vertexScratch; - vertexScratch.resize(static_cast(hr.mVcount)); - - BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); - - ret = QE_OK; - - if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! - { - result.mPolygons = false; - result.mNumOutputVertices = ovcount; - result.m_OutputVertices.resize(static_cast(ovcount)); - result.mNumFaces = hr.mFaceCount; - result.mNumIndices = hr.mIndexCount; - - result.m_Indices.resize(static_cast(hr.mIndexCount)); - - memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); - - if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) - { - - const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; - - for (unsigned int i=0; i(ovcount)); - result.mNumFaces = hr.mFaceCount; - result.mNumIndices = hr.mIndexCount+hr.mFaceCount; - result.m_Indices.resize(static_cast(result.mNumIndices)); - memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); - -// if ( 1 ) - { - const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; - for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; - } - } - } - - btScalar dx = bmax[0] - bmin[0]; - btScalar dy = bmax[1] - bmin[1]; - btScalar dz = bmax[2] - bmin[2]; - - btVector3 center; - - center[0] = dx*btScalar(0.5) + bmin[0]; - center[1] = dy*btScalar(0.5) + bmin[1]; - center[2] = dz*btScalar(0.5) + bmin[2]; - - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) - { - - btScalar len = FLT_MAX; - - if ( dx > EPSILON && dx < len ) len = dx; - if ( dy > EPSILON && dy < len ) len = dy; - if ( dz > EPSILON && dz < len ) len = dz; - - if ( len == FLT_MAX ) - { - dx = dy = dz = btScalar(0.01); // one centimeter - } - else - { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); - } - - btScalar x1 = center[0] - dx; - btScalar x2 = center[0] + dx; - - btScalar y1 = center[1] - dy; - btScalar y2 = center[1] + dy; - - btScalar z1 = center[2] - dz; - btScalar z2 = center[2] + dz; - - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); - - return true; // return cube - - - } - else - { - if ( scale ) - { - scale[0] = dx; - scale[1] = dy; - scale[2] = dz; - - recip[0] = 1 / dx; - recip[1] = 1 / dy; - recip[2] = 1 / dz; - - center[0]*=recip[0]; - center[1]*=recip[1]; - center[2]*=recip[2]; - - } - - } - - - - vtx = (const char *) svertices; - - for (unsigned int i=0; igetX(); - btScalar py = p->getY(); - btScalar pz = p->getZ(); - - if ( scale ) - { - px = px*recip[0]; // normalize - py = py*recip[1]; // normalize - pz = pz*recip[2]; // normalize - } - -// if ( 1 ) - { - unsigned int j; - - for (j=0; j dist2 ) - { - v[0] = px; - v[1] = py; - v[2] = pz; - - } - - break; - } - } - - if ( j == vcount ) - { - btVector3& dest = vertices[vcount]; - dest[0] = px; - dest[1] = py; - dest[2] = pz; - vcount++; - } - m_vertexIndexMapping.push_back(j); - } - } - - // ok..now make sure we didn't prune so many vertices it is now invalid. -// if ( 1 ) - { - btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; - btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; - - for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; - } - } - - btScalar dx = bmax[0] - bmin[0]; - btScalar dy = bmax[1] - bmin[1]; - btScalar dz = bmax[2] - bmin[2]; - - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) - { - btScalar cx = dx*btScalar(0.5) + bmin[0]; - btScalar cy = dy*btScalar(0.5) + bmin[1]; - btScalar cz = dz*btScalar(0.5) + bmin[2]; - - btScalar len = FLT_MAX; - - if ( dx >= EPSILON && dx < len ) len = dx; - if ( dy >= EPSILON && dy < len ) len = dy; - if ( dz >= EPSILON && dz < len ) len = dz; - - if ( len == FLT_MAX ) - { - dx = dy = dz = btScalar(0.01); // one centimeter - } - else - { - if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * btScalar(0.05); - if ( dz < EPSILON ) dz = len * btScalar(0.05); - } - - btScalar x1 = cx - dx; - btScalar x2 = cx + dx; - - btScalar y1 = cy - dy; - btScalar y2 = cy + dy; - - btScalar z1 = cz - dz; - btScalar z2 = cz + dz; - - vcount = 0; // add box - - addPoint(vcount,vertices,x1,y1,z1); - addPoint(vcount,vertices,x2,y1,z1); - addPoint(vcount,vertices,x2,y2,z1); - addPoint(vcount,vertices,x1,y2,z1); - addPoint(vcount,vertices,x1,y1,z2); - addPoint(vcount,vertices,x2,y1,z2); - addPoint(vcount,vertices,x2,y2,z2); - addPoint(vcount,vertices,x1,y2,z2); - - return true; - } - } - - return true; -} - -void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) -{ - btAlignedObjectArraytmpIndices; - tmpIndices.resize(m_vertexIndexMapping.size()); - int i; - - for (i=0;i(vcount)); - memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); - - ocount = 0; - - for (i=0; i= 0 && v < vcount ); - - if ( usedIndices[static_cast(v)] ) // if already remapped - { - indices[i] = usedIndices[static_cast(v)]-1; // index to new array - } - else - { - - indices[i] = ocount; // new index mapping - - overts[ocount][0] = verts[v][0]; // copy old vert to new vert array - overts[ocount][1] = verts[v][1]; - overts[ocount][2] = verts[v][2]; - - for (int k=0;k=0 && ocount <= vcount ); - - usedIndices[static_cast(v)] = ocount; // assign new index remapping - - - } - } - - -} +/* +Stan Melax Convex Hull Computation +Copyright (c) 2003-2006 Stan Melax http://www.melax.com/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include + +#include "btConvexHull.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btVector3.h" + + + +template +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + +//---------------------------------- + +class int3 +{ +public: + int x,y,z; + int3(){}; + int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + + +//------- btPlane ---------- + + +inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} +inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } +inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } + + +//--------- Utility Functions ------ + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); + +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) +{ + btVector3 N1 = p0.normal; + btVector3 N2 = p1.normal; + btVector3 N3 = p2.normal; + + btVector3 n2n3; n2n3 = N2.cross(N3); + btVector3 n3n1; n3n1 = N3.cross(N1); + btVector3 n1n2; n1n2 = N1.cross(N2); + + btScalar quotient = (N1.dot(n2n3)); + + btAssert(btFabs(quotient) > btScalar(0.000001)); + + quotient = btScalar(-1.) / quotient; + n2n3 *= p0.dist; + n3n1 *= p1.dist; + n1n2 *= p2.dist; + btVector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + return result; + +} + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); +btVector3 NormalOf(const btVector3 *vert, const int n); + + +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) +{ + // returns the point where the line p0-p1 intersects the plane n&d + static btVector3 dif; + dif = p1-p0; + btScalar dn= dot(plane.normal,dif); + btScalar t = -(plane.dist+dot(plane.normal,p0) )/dn; + return p0 + (dif*t); +} + +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) +{ + return point - plane.normal * (dot(point,plane.normal)+plane.dist); +} + +btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) +{ + // return the normal of the triangle + // inscribed by v0, v1, and v2 + btVector3 cp=cross(v1-v0,v2-v1); + btScalar m=cp.length(); + if(m==0) return btVector3(1,0,0); + return cp*(btScalar(1.0)/m); +} + + +btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) +{ + static btVector3 cp; + cp = cross(udir,vdir).normalized(); + + btScalar distu = -dot(cp,ustart); + btScalar distv = -dot(cp,vstart); + btScalar dist = (btScalar)fabs(distu-distv); + if(upoint) + { + btPlane plane; + plane.normal = cross(vdir,cp).normalized(); + plane.dist = -dot(plane.normal,vstart); + *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); + } + if(vpoint) + { + btPlane plane; + plane.normal = cross(udir,cp).normalized(); + plane.dist = -dot(plane.normal,ustart); + *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + } + return dist; +} + + + + + + + +#define COPLANAR (0) +#define UNDER (1) +#define OVER (2) +#define SPLIT (OVER|UNDER) +#define PAPERWIDTH (btScalar(0.001)) + +btScalar planetestepsilon = PAPERWIDTH; + + + +typedef ConvexH::HalfEdge HalfEdge; + +ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) +{ + vertices.resize(vertices_size); + edges.resize(edges_size); + facets.resize(facets_size); +} + + +int PlaneTest(const btPlane &p, const btVector3 &v); +int PlaneTest(const btPlane &p, const btVector3 &v) { + btScalar a = dot(v,p.normal)+p.dist; + int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); + return flag; +} + +int SplitTest(ConvexH &convex,const btPlane &plane); +int SplitTest(ConvexH &convex,const btPlane &plane) { + int flag=0; + for(int i=0;i +int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +{ + btAssert(count); + int m=-1; + for(int i=0;idot(p[m],dir)) + m=i; + } + btAssert(m!=-1); + return m; +} + +btVector3 orth(const btVector3 &v); +btVector3 orth(const btVector3 &v) +{ + btVector3 a=cross(v,btVector3(0,0,1)); + btVector3 b=cross(v,btVector3(0,1,0)); + if (a.length() > b.length()) + { + return a.normalized(); + } else { + return b.normalized(); + } +} + + +template +int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &allow) +{ + int m=-1; + while(m==-1) + { + m = maxdirfiltered(p,count,dir,allow); + if(allow[m]==3) return m; + T u = orth(dir); + T v = cross(u,dir); + int ma=-1; + for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) + { + btScalar s = sinf(SIMD_RADS_PER_DEG*(x)); + btScalar c = cosf(SIMD_RADS_PER_DEG*(x)); + int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(ma==m && mb==m) + { + allow[m]=3; + return m; + } + if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + { + int mc = ma; + for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) + { + btScalar s = sinf(SIMD_RADS_PER_DEG*(xx)); + btScalar c = cosf(SIMD_RADS_PER_DEG*(xx)); + int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); + if(mc==m && md==m) + { + allow[m]=3; + return m; + } + mc=md; + } + } + ma=mb; + } + allow[m]=0; + m=-1; + } + btAssert(0); + return m; +} + + + + +int operator ==(const int3 &a,const int3 &b); +int operator ==(const int3 &a,const int3 &b) +{ + for(int i=0;i<3;i++) + { + if(a[i]!=b[i]) return 0; + } + return 1; +} + + +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) +{ + btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); + return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? +} +int hasedge(const int3 &t, int a,int b); +int hasedge(const int3 &t, int a,int b) +{ + for(int i=0;i<3;i++) + { + int i1= (i+1)%3; + if(t[i]==a && t[i1]==b) return 1; + } + return 0; +} +int hasvert(const int3 &t, int v); +int hasvert(const int3 &t, int v) +{ + return (t[0]==v || t[1]==v || t[2]==v) ; +} +int shareedge(const int3 &a,const int3 &b); +int shareedge(const int3 &a,const int3 &b) +{ + int i; + for(i=0;i<3;i++) + { + int i1= (i+1)%3; + if(hasedge(a,b[i1],b[i])) return 1; + } + return 0; +} + +class btHullTriangle; + + + +class btHullTriangle : public int3 +{ +public: + int3 n; + int id; + int vmax; + btScalar rise; + btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + { + vmax=-1; + rise = btScalar(0.0); + } + ~btHullTriangle() + { + } + int &neib(int a,int b); +}; + + +int &btHullTriangle::neib(int a,int b) +{ + static int er=-1; + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + if((*this)[i]==a && (*this)[i1]==b) return n[i2]; + if((*this)[i]==b && (*this)[i1]==a) return n[i2]; + } + btAssert(0); + return er; +} +void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) +{ + int i; + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*s)[i1]; + int b = (*s)[i2]; + btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); + btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); + m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); + m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + } +} + +void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) +{ + b2bfix(s,t); + deAllocateTriangle(s); + + deAllocateTriangle(t); +} + +void HullLibrary::checkit(btHullTriangle *t) +{ + (void)t; + + int i; + btAssert(m_tris[t->id]==t); + for(i=0;i<3;i++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int a = (*t)[i1]; + int b = (*t)[i2]; + + // release compile fix + (void)i1; + (void)i2; + (void)a; + (void)b; + + btAssert(a!=b); + btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); + } +} + +btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) +{ + void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); + btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); + tr->id = m_tris.size(); + m_tris.push_back(tr); + + return tr; +} + +void HullLibrary::deAllocateTriangle(btHullTriangle* tri) +{ + btAssert(m_tris[tri->id]==tri); + m_tris[tri->id]=NULL; + tri->~btHullTriangle(); + btAlignedFree(tri); +} + + +void HullLibrary::extrude(btHullTriangle *t0,int v) +{ + int3 t= *t0; + int n = m_tris.size(); + btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); + ta->n = int3(t0->n[0],n+1,n+2); + m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; + btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); + tb->n = int3(t0->n[1],n+2,n+0); + m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; + btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); + tc->n = int3(t0->n[2],n+0,n+1); + m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + checkit(ta); + checkit(tb); + checkit(tc); + if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); + if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); + if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); + deAllocateTriangle(t0); + +} + +btHullTriangle* HullLibrary::extrudable(btScalar epsilon) +{ + int i; + btHullTriangle *t=NULL; + for(i=0;iriserise)) + { + t = m_tris[i]; + } + } + return (t->rise >epsilon)?t:NULL ; +} + + + + +int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) +{ + btVector3 basis[3]; + basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); + int p0 = maxdirsterid(verts,verts_count, basis[0],allow); + int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); + basis[0] = verts[p0]-verts[p1]; + if(p0==p1 || basis[0]==btVector3(0,0,0)) + return int4(-1,-1,-1,-1); + basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); + basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + if (basis[1].length() > basis[2].length()) + { + basis[1].normalize(); + } else { + basis[1] = basis[2]; + basis[1].normalize (); + } + int p2 = maxdirsterid(verts,verts_count,basis[1],allow); + if(p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + } + if(p2 == p0 || p2 == p1) + return int4(-1,-1,-1,-1); + basis[1] = verts[p2] - verts[p0]; + basis[2] = cross(basis[1],basis[0]).normalized(); + int p3 = maxdirsterid(verts,verts_count,basis[2],allow); + if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); + if(p3==p0||p3==p1||p3==p2) + return int4(-1,-1,-1,-1); + btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); + if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} + return int4(p0,p1,p2,p3); +} + +int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) +{ + if(verts_count <4) return 0; + if(vlimit==0) vlimit=1000000000; + int j; + btVector3 bmin(*verts),bmax(*verts); + btAlignedObjectArray isextreme; + isextreme.reserve(verts_count); + btAlignedObjectArray allow; + allow.reserve(verts_count); + + for(j=0;jn=int3(2,3,1); + btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); + btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); + btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); + isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; + checkit(t0);checkit(t1);checkit(t2);checkit(t3); + + for(j=0;jvmax<0); + btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + btHullTriangle *te; + vlimit-=4; + while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) + { + int3 ti=*te; + int v=te->vmax; + btAssert(v != -1); + btAssert(!isextreme[v]); // wtf we've already done this vertex + isextreme[v]=1; + //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already + j=m_tris.size(); + while(j--) { + if(!m_tris[j]) continue; + int3 t=*m_tris[j]; + if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) + { + extrude(m_tris[j],v); + } + } + // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle + j=m_tris.size(); + while(j--) + { + if(!m_tris[j]) continue; + if(!hasvert(*m_tris[j],v)) break; + int3 nt=*m_tris[j]; + if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) + { + btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; + btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; + btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); + t->vmax = maxdirsterid(verts,verts_count,n,allow); + if(isextreme[t->vmax]) + { + t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. + } + else + { + t->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); + } + } + vlimit --; + } + return 1; +} + +int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) +{ + int rc=calchullgen(verts,verts_count, vlimit) ; + if(!rc) return 0; + btAlignedObjectArray ts; + int i; + + for(i=0;i(ts[i]); + } + m_tris.resize(0); + + return 1; +} + + + + + +bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) +{ + + int tris_count; + int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast(vlimit) ); + if(!ret) return false; + result.mIndexCount = (unsigned int) (tris_count*3); + result.mFaceCount = (unsigned int) tris_count; + result.mVertices = (btVector3*) vertices; + result.mVcount = (unsigned int) vcount; + return true; + +} + + +void ReleaseHull(PHullResult &result); +void ReleaseHull(PHullResult &result) +{ + if ( result.m_Indices.size() ) + { + result.m_Indices.clear(); + } + + result.mVcount = 0; + result.mIndexCount = 0; + result.mVertices = 0; +} + + +//********************************************************************* +//********************************************************************* +//******** HullLib header +//********************************************************************* +//********************************************************************* + +//********************************************************************* +//********************************************************************* +//******** HullLib implementation +//********************************************************************* +//********************************************************************* + +HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request + HullResult &result) // contains the resulst +{ + HullError ret = QE_FAIL; + + + PHullResult hr; + + unsigned int vcount = desc.mVcount; + if ( vcount < 8 ) vcount = 8; + + btAlignedObjectArray vertexSource; + vertexSource.resize(static_cast(vcount)); + + btVector3 scale; + + unsigned int ovcount; + + bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + + if ( ok ) + { + + +// if ( 1 ) // scale vertices back to their original size. + { + for (unsigned int i=0; i(i)]; + v[0]*=scale[0]; + v[1]*=scale[1]; + v[2]*=scale[2]; + } + } + + ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); + + if ( ok ) + { + + // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. + btAlignedObjectArray vertexScratch; + vertexScratch.resize(static_cast(hr.mVcount)); + + BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); + + ret = QE_OK; + + if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! + { + result.mPolygons = false; + result.mNumOutputVertices = ovcount; + result.m_OutputVertices.resize(static_cast(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount; + + result.m_Indices.resize(static_cast(hr.mIndexCount)); + + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + + if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + { + + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + + for (unsigned int i=0; i(ovcount)); + result.mNumFaces = hr.mFaceCount; + result.mNumIndices = hr.mIndexCount+hr.mFaceCount; + result.m_Indices.resize(static_cast(result.mNumIndices)); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); + +// if ( 1 ) + { + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; + for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + } + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + btVector3 center; + + center[0] = dx*btScalar(0.5) + bmin[0]; + center[1] = dy*btScalar(0.5) + bmin[1]; + center[2] = dz*btScalar(0.5) + bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) + { + + btScalar len = FLT_MAX; + + if ( dx > EPSILON && dx < len ) len = dx; + if ( dy > EPSILON && dy < len ) len = dy; + if ( dz > EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = center[0] - dx; + btScalar x2 = center[0] + dx; + + btScalar y1 = center[1] - dy; + btScalar y2 = center[1] + dy; + + btScalar z1 = center[2] - dz; + btScalar z2 = center[2] + dz; + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; // return cube + + + } + else + { + if ( scale ) + { + scale[0] = dx; + scale[1] = dy; + scale[2] = dz; + + recip[0] = 1 / dx; + recip[1] = 1 / dy; + recip[2] = 1 / dz; + + center[0]*=recip[0]; + center[1]*=recip[1]; + center[2]*=recip[2]; + + } + + } + + + + vtx = (const char *) svertices; + + for (unsigned int i=0; igetX(); + btScalar py = p->getY(); + btScalar pz = p->getZ(); + + if ( scale ) + { + px = px*recip[0]; // normalize + py = py*recip[1]; // normalize + pz = pz*recip[2]; // normalize + } + +// if ( 1 ) + { + unsigned int j; + + for (j=0; j dist2 ) + { + v[0] = px; + v[1] = py; + v[2] = pz; + + } + + break; + } + } + + if ( j == vcount ) + { + btVector3& dest = vertices[vcount]; + dest[0] = px; + dest[1] = py; + dest[2] = pz; + vcount++; + } + m_vertexIndexMapping.push_back(j); + } + } + + // ok..now make sure we didn't prune so many vertices it is now invalid. +// if ( 1 ) + { + btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; + btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + + for (unsigned int i=0; i bmax[j] ) bmax[j] = p[j]; + } + } + + btScalar dx = bmax[0] - bmin[0]; + btScalar dy = bmax[1] - bmin[1]; + btScalar dz = bmax[2] - bmin[2]; + + if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + { + btScalar cx = dx*btScalar(0.5) + bmin[0]; + btScalar cy = dy*btScalar(0.5) + bmin[1]; + btScalar cz = dz*btScalar(0.5) + bmin[2]; + + btScalar len = FLT_MAX; + + if ( dx >= EPSILON && dx < len ) len = dx; + if ( dy >= EPSILON && dy < len ) len = dy; + if ( dz >= EPSILON && dz < len ) len = dz; + + if ( len == FLT_MAX ) + { + dx = dy = dz = btScalar(0.01); // one centimeter + } + else + { + if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. + if ( dy < EPSILON ) dy = len * btScalar(0.05); + if ( dz < EPSILON ) dz = len * btScalar(0.05); + } + + btScalar x1 = cx - dx; + btScalar x2 = cx + dx; + + btScalar y1 = cy - dy; + btScalar y2 = cy + dy; + + btScalar z1 = cz - dz; + btScalar z2 = cz + dz; + + vcount = 0; // add box + + addPoint(vcount,vertices,x1,y1,z1); + addPoint(vcount,vertices,x2,y1,z1); + addPoint(vcount,vertices,x2,y2,z1); + addPoint(vcount,vertices,x1,y2,z1); + addPoint(vcount,vertices,x1,y1,z2); + addPoint(vcount,vertices,x2,y1,z2); + addPoint(vcount,vertices,x2,y2,z2); + addPoint(vcount,vertices,x1,y2,z2); + + return true; + } + } + + return true; +} + +void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +{ + btAlignedObjectArraytmpIndices; + tmpIndices.resize(m_vertexIndexMapping.size()); + int i; + + for (i=0;i(vcount)); + memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); + + ocount = 0; + + for (i=0; i= 0 && v < vcount ); + + if ( usedIndices[static_cast(v)] ) // if already remapped + { + indices[i] = usedIndices[static_cast(v)]-1; // index to new array + } + else + { + + indices[i] = ocount; // new index mapping + + overts[ocount][0] = verts[v][0]; // copy old vert to new vert array + overts[ocount][1] = verts[v][1]; + overts[ocount][2] = verts[v][2]; + + for (int k=0;k=0 && ocount <= vcount ); + + usedIndices[static_cast(v)] = ocount; // assign new index remapping + + + } + } + + +} diff --git a/src/LinearMath/btConvexHull.h b/src/LinearMath/btConvexHull.h index b82e5f0e8..92560bddb 100644 --- a/src/LinearMath/btConvexHull.h +++ b/src/LinearMath/btConvexHull.h @@ -1,241 +1,241 @@ - -/* -Stan Melax Convex Hull Computation -Copyright (c) 2008 Stan Melax http://www.melax.com/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. - -#ifndef CD_HULL_H -#define CD_HULL_H - -#include "LinearMath/btVector3.h" -#include "LinearMath/btAlignedObjectArray.h" - -typedef btAlignedObjectArray TUIntArray; - -class HullResult -{ -public: - HullResult(void) - { - mPolygons = true; - mNumOutputVertices = 0; - mNumFaces = 0; - mNumIndices = 0; - } - bool mPolygons; // true if indices represents polygons, false indices are triangles - unsigned int mNumOutputVertices; // number of vertices in the output hull - btAlignedObjectArray m_OutputVertices; // array of vertices - unsigned int mNumFaces; // the number of faces produced - unsigned int mNumIndices; // the total number of indices - btAlignedObjectArray m_Indices; // pointer to indices. - -// If triangles, then indices are array indexes into the vertex list. -// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. -}; - -enum HullFlag -{ - QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. - QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. - QF_DEFAULT = QF_TRIANGLES -}; - - -class HullDesc -{ -public: - HullDesc(void) - { - mFlags = QF_DEFAULT; - mVcount = 0; - mVertices = 0; - mVertexStride = sizeof(btVector3); - mNormalEpsilon = 0.001f; - mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. - mMaxFaces = 4096; - }; - - HullDesc(HullFlag flag, - unsigned int vcount, - const btVector3 *vertices, - unsigned int stride = sizeof(btVector3)) - { - mFlags = flag; - mVcount = vcount; - mVertices = vertices; - mVertexStride = stride; - mNormalEpsilon = btScalar(0.001); - mMaxVertices = 4096; - } - - bool HasHullFlag(HullFlag flag) const - { - if ( mFlags & flag ) return true; - return false; - } - - void SetHullFlag(HullFlag flag) - { - mFlags|=flag; - } - - void ClearHullFlag(HullFlag flag) - { - mFlags&=~flag; - } - - unsigned int mFlags; // flags to use when generating the convex hull. - unsigned int mVcount; // number of vertices in the input point cloud - const btVector3 *mVertices; // the array of vertices. - unsigned int mVertexStride; // the stride of each vertex, in bytes. - btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. - unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! - unsigned int mMaxFaces; -}; - -enum HullError -{ - QE_OK, // success! - QE_FAIL // failed. -}; - -class btPlane -{ - public: - btVector3 normal; - btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 - btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} - btPlane():normal(),dist(0){} - -}; - - - -class ConvexH -{ - public: - class HalfEdge - { - public: - short ea; // the other half of the edge (index into edges list) - unsigned char v; // the vertex at the start of this edge (index into vertices list) - unsigned char p; // the facet on which this edge lies (index into facets list) - HalfEdge(){} - HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} - }; - ConvexH() - { - } - ~ConvexH() - { - } - btAlignedObjectArray vertices; - btAlignedObjectArray edges; - btAlignedObjectArray facets; - ConvexH(int vertices_size,int edges_size,int facets_size); -}; - - -class int4 -{ -public: - int x,y,z,w; - int4(){}; - int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} - const int& operator[](int i) const {return (&x)[i];} - int& operator[](int i) {return (&x)[i];} -}; - -class PHullResult -{ -public: - - PHullResult(void) - { - mVcount = 0; - mIndexCount = 0; - mFaceCount = 0; - mVertices = 0; - } - - unsigned int mVcount; - unsigned int mIndexCount; - unsigned int mFaceCount; - btVector3* mVertices; - TUIntArray m_Indices; -}; - - - -///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. -///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. -class HullLibrary -{ - - btAlignedObjectArray m_tris; - -public: - - btAlignedObjectArray m_vertexIndexMapping; - - - HullError CreateConvexHull(const HullDesc& desc, // describes the input request - HullResult& result); // contains the resulst - HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. - -private: - - bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); - - class btHullTriangle* allocateTriangle(int a,int b,int c); - void deAllocateTriangle(btHullTriangle*); - void b2bfix(btHullTriangle* s,btHullTriangle*t); - - void removeb2b(btHullTriangle* s,btHullTriangle*t); - - void checkit(btHullTriangle *t); - - btHullTriangle* extrudable(btScalar epsilon); - - int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); - - int calchullgen(btVector3 *verts,int verts_count, int vlimit); - - int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); - - class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); - - void extrude(class btHullTriangle* t0,int v); - - ConvexH* test_cube(); - - //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. - //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. - //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. - //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. - void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); - - bool CleanupVertices(unsigned int svcount, - const btVector3* svertices, - unsigned int stride, - unsigned int &vcount, // output number of vertices - btVector3* vertices, // location to store the results. - btScalar normalepsilon, - btVector3& scale); -}; - - -#endif - + +/* +Stan Melax Convex Hull Computation +Copyright (c) 2008 Stan Melax http://www.melax.com/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. + +#ifndef CD_HULL_H +#define CD_HULL_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +typedef btAlignedObjectArray TUIntArray; + +class HullResult +{ +public: + HullResult(void) + { + mPolygons = true; + mNumOutputVertices = 0; + mNumFaces = 0; + mNumIndices = 0; + } + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + btAlignedObjectArray m_OutputVertices; // array of vertices + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + btAlignedObjectArray m_Indices; // pointer to indices. + +// If triangles, then indices are array indexes into the vertex list. +// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. +}; + +enum HullFlag +{ + QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. + QF_DEFAULT = QF_TRIANGLES +}; + + +class HullDesc +{ +public: + HullDesc(void) + { + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(btVector3); + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; + }; + + HullDesc(HullFlag flag, + unsigned int vcount, + const btVector3 *vertices, + unsigned int stride = sizeof(btVector3)) + { + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = btScalar(0.001); + mMaxVertices = 4096; + } + + bool HasHullFlag(HullFlag flag) const + { + if ( mFlags & flag ) return true; + return false; + } + + void SetHullFlag(HullFlag flag) + { + mFlags|=flag; + } + + void ClearHullFlag(HullFlag flag) + { + mFlags&=~flag; + } + + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const btVector3 *mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; +}; + +enum HullError +{ + QE_OK, // success! + QE_FAIL // failed. +}; + +class btPlane +{ + public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} + btPlane():normal(),dist(0){} + +}; + + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + ConvexH() + { + } + ~ConvexH() + { + } + btAlignedObjectArray vertices; + btAlignedObjectArray edges; + btAlignedObjectArray facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + btVector3* mVertices; + TUIntArray m_Indices; +}; + + + +///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. +///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. +class HullLibrary +{ + + btAlignedObjectArray m_tris; + +public: + + btAlignedObjectArray m_vertexIndexMapping; + + + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst + HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + +private: + + bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); + + class btHullTriangle* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(btHullTriangle*); + void b2bfix(btHullTriangle* s,btHullTriangle*t); + + void removeb2b(btHullTriangle* s,btHullTriangle*t); + + void checkit(btHullTriangle *t); + + btHullTriangle* extrudable(btScalar epsilon); + + int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + + int calchullgen(btVector3 *verts,int verts_count, int vlimit); + + int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); + + class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + + void extrude(class btHullTriangle* t0,int v); + + ConvexH* test_cube(); + + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. + //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. + //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. + //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. + void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); + + bool CleanupVertices(unsigned int svcount, + const btVector3* svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + btVector3* vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale); +}; + + +#endif + diff --git a/src/LinearMath/btDefaultMotionState.h b/src/LinearMath/btDefaultMotionState.h index f38be1bd2..d758f77ed 100644 --- a/src/LinearMath/btDefaultMotionState.h +++ b/src/LinearMath/btDefaultMotionState.h @@ -1,38 +1,38 @@ -#ifndef DEFAULT_MOTION_STATE_H -#define DEFAULT_MOTION_STATE_H - -///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. -struct btDefaultMotionState : public btMotionState -{ - btTransform m_graphicsWorldTrans; - btTransform m_centerOfMassOffset; - btTransform m_startWorldTrans; - void* m_userPointer; - - btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity()) - : m_graphicsWorldTrans(startTrans), - m_centerOfMassOffset(centerOfMassOffset), - m_startWorldTrans(startTrans), - m_userPointer(0) - - { - } - - ///synchronizes world transform from user to physics - virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const - { - centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; - } - - ///synchronizes world transform from physics to user - ///Bullet only calls the update of worldtransform for active objects - virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) - { - m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; - } - - - -}; - -#endif //DEFAULT_MOTION_STATE_H +#ifndef DEFAULT_MOTION_STATE_H +#define DEFAULT_MOTION_STATE_H + +///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets. +struct btDefaultMotionState : public btMotionState +{ + btTransform m_graphicsWorldTrans; + btTransform m_centerOfMassOffset; + btTransform m_startWorldTrans; + void* m_userPointer; + + btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity()) + : m_graphicsWorldTrans(startTrans), + m_centerOfMassOffset(centerOfMassOffset), + m_startWorldTrans(startTrans), + m_userPointer(0) + + { + } + + ///synchronizes world transform from user to physics + virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const + { + centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; + } + + ///synchronizes world transform from physics to user + ///Bullet only calls the update of worldtransform for active objects + virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) + { + m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; + } + + + +}; + +#endif //DEFAULT_MOTION_STATE_H diff --git a/src/LinearMath/btGeometryUtil.cpp b/src/LinearMath/btGeometryUtil.cpp index 5fce2e150..5ac230f71 100644 --- a/src/LinearMath/btGeometryUtil.cpp +++ b/src/LinearMath/btGeometryUtil.cpp @@ -1,185 +1,185 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btGeometryUtil.h" - - -/* - Make sure this dummy function never changes so that it - can be used by probes that are checking whether the - library is actually installed. -*/ -extern "C" -{ - void btBulletMathProbe (); - - void btBulletMathProbe () {} -} - - -bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) -{ - int numbrushes = planeEquations.size(); - for (int i=0;ibtScalar(0.)) - { - return false; - } - } - return true; - -} - - -bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) -{ - int numvertices = vertices.size(); - for (int i=0;ibtScalar(0.)) - { - return false; - } - } - return true; -} - -bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations); - -bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations) -{ - int numbrushes = planeEquations.size(); - for (int i=0;i btScalar(0.999)) - { - return false; - } - } - return true; -} - -void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ) -{ - const int numvertices = vertices.size(); - // brute force: - for (int i=0;i btScalar(0.0001)) - { - planeEquation.normalize(); - if (notExist(planeEquation,planeEquationsOut)) - { - planeEquation[3] = -planeEquation.dot(N1); - - //check if inside, and replace supportingVertexOut if needed - if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) - { - planeEquationsOut.push_back(planeEquation); - } - } - } - normalSign = btScalar(-1.); - } - - } - } - } - -} - -void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ) -{ - const int numbrushes = planeEquations.size(); - // brute force: - for (int i=0;i btScalar(0.0001) ) && - ( n3n1.length2() > btScalar(0.0001) ) && - ( n1n2.length2() > btScalar(0.0001) ) ) - { - //point P out of 3 plane equations: - - // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) - //P = ------------------------------------------------------------------------- - // N1 . ( N2 * N3 ) - - - btScalar quotient = (N1.dot(n2n3)); - if (btFabs(quotient) > btScalar(0.000001)) - { - quotient = btScalar(-1.) / quotient; - n2n3 *= N1[3]; - n3n1 *= N2[3]; - n1n2 *= N3[3]; - btVector3 potentialVertex = n2n3; - potentialVertex += n3n1; - potentialVertex += n1n2; - potentialVertex *= quotient; - - //check if inside, and replace supportingVertexOut if needed - if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01))) - { - verticesOut.push_back(potentialVertex); - } - } - } - } - } - } -} - +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btGeometryUtil.h" + + +/* + Make sure this dummy function never changes so that it + can be used by probes that are checking whether the + library is actually installed. +*/ +extern "C" +{ + void btBulletMathProbe (); + + void btBulletMathProbe () {} +} + + +bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin) +{ + int numbrushes = planeEquations.size(); + for (int i=0;ibtScalar(0.)) + { + return false; + } + } + return true; + +} + + +bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin) +{ + int numvertices = vertices.size(); + for (int i=0;ibtScalar(0.)) + { + return false; + } + } + return true; +} + +bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations); + +bool notExist(const btVector3& planeEquation,const btAlignedObjectArray& planeEquations) +{ + int numbrushes = planeEquations.size(); + for (int i=0;i btScalar(0.999)) + { + return false; + } + } + return true; +} + +void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ) +{ + const int numvertices = vertices.size(); + // brute force: + for (int i=0;i btScalar(0.0001)) + { + planeEquation.normalize(); + if (notExist(planeEquation,planeEquationsOut)) + { + planeEquation[3] = -planeEquation.dot(N1); + + //check if inside, and replace supportingVertexOut if needed + if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) + { + planeEquationsOut.push_back(planeEquation); + } + } + } + normalSign = btScalar(-1.); + } + + } + } + } + +} + +void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ) +{ + const int numbrushes = planeEquations.size(); + // brute force: + for (int i=0;i btScalar(0.0001) ) && + ( n3n1.length2() > btScalar(0.0001) ) && + ( n1n2.length2() > btScalar(0.0001) ) ) + { + //point P out of 3 plane equations: + + // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + //P = ------------------------------------------------------------------------- + // N1 . ( N2 * N3 ) + + + btScalar quotient = (N1.dot(n2n3)); + if (btFabs(quotient) > btScalar(0.000001)) + { + quotient = btScalar(-1.) / quotient; + n2n3 *= N1[3]; + n3n1 *= N2[3]; + n1n2 *= N3[3]; + btVector3 potentialVertex = n2n3; + potentialVertex += n3n1; + potentialVertex += n1n2; + potentialVertex *= quotient; + + //check if inside, and replace supportingVertexOut if needed + if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01))) + { + verticesOut.push_back(potentialVertex); + } + } + } + } + } + } +} + diff --git a/src/LinearMath/btGeometryUtil.h b/src/LinearMath/btGeometryUtil.h index c18d6835f..a4b13b456 100644 --- a/src/LinearMath/btGeometryUtil.h +++ b/src/LinearMath/btGeometryUtil.h @@ -1,42 +1,42 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef BT_GEOMETRY_UTIL_H -#define BT_GEOMETRY_UTIL_H - -#include "btVector3.h" -#include "btAlignedObjectArray.h" - -///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices. -class btGeometryUtil -{ - public: - - - static void getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ); - - static void getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ); - - static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, btScalar margin); - - static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin); - - static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); - -}; - - -#endif //BT_GEOMETRY_UTIL_H - +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef BT_GEOMETRY_UTIL_H +#define BT_GEOMETRY_UTIL_H + +#include "btVector3.h" +#include "btAlignedObjectArray.h" + +///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices. +class btGeometryUtil +{ + public: + + + static void getPlaneEquationsFromVertices(btAlignedObjectArray& vertices, btAlignedObjectArray& planeEquationsOut ); + + static void getVerticesFromPlaneEquations(const btAlignedObjectArray& planeEquations , btAlignedObjectArray& verticesOut ); + + static bool isInside(const btAlignedObjectArray& vertices, const btVector3& planeNormal, btScalar margin); + + static bool isPointInsidePlanes(const btAlignedObjectArray& planeEquations, const btVector3& point, btScalar margin); + + static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray& vertices, btScalar margin); + +}; + + +#endif //BT_GEOMETRY_UTIL_H + diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index b1ce469af..f883e0e48 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -1,303 +1,303 @@ -#ifndef BT_HASH_MAP_H -#define BT_HASH_MAP_H - -#include "btAlignedObjectArray.h" - -const int BT_HASH_NULL=0xffffffff; - -template -class btHashKey -{ - int m_uid; -public: - - btHashKey(int uid) - :m_uid(uid) - { - } - - int getUid() const - { - return m_uid; - } - - //to our success - SIMD_FORCE_INLINE unsigned int getHash()const - { - int key = m_uid; - // Thomas Wang's hash - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); - return key; - } - - btHashKey getKey(const Value& value) const - { - return btHashKey(value.getUid()); - } -}; - - -template -class btHashKeyPtr -{ - int m_uid; -public: - - btHashKeyPtr(int uid) - :m_uid(uid) - { - } - - int getUid() const - { - return m_uid; - } - - //to our success - SIMD_FORCE_INLINE unsigned int getHash()const - { - int key = m_uid; - // Thomas Wang's hash - key += ~(key << 15); - key ^= (key >> 10); - key += (key << 3); - key ^= (key >> 6); - key += ~(key << 11); - key ^= (key >> 16); - return key; - } - - btHashKeyPtr getKey(const Value& value) const - { - return btHashKeyPtr(value->getUid()); - } -}; - -///The btHashMap template class implements a generic and lightweight hashmap. -///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp -template -class btHashMap -{ - - btAlignedObjectArray m_hashTable; - btAlignedObjectArray m_next; - btAlignedObjectArray m_valueArray; - - - - void growTables(const Key& key) - { - int newCapacity = m_valueArray.capacity(); - - if (m_hashTable.size() < newCapacity) - { - //grow hashtable and next table - int curHashtableSize = m_hashTable.size(); - - m_hashTable.resize(newCapacity); - m_next.resize(newCapacity); - - int i; - - for (i= 0; i < newCapacity; ++i) - { - m_hashTable[i] = BT_HASH_NULL; - } - for (i = 0; i < newCapacity; ++i) - { - m_next[i] = BT_HASH_NULL; - } - - for(i=0;i= m_hashTable.size()) - { - return BT_HASH_NULL; - } - - int index = m_hashTable[hash]; - while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false) - { - index = m_next[index]; - } - return index; - } - - void clear() - { - m_hashTable.clear(); - m_next.clear(); - m_valueArray.clear(); - } - -}; - -#endif //BT_HASH_MAP_H +#ifndef BT_HASH_MAP_H +#define BT_HASH_MAP_H + +#include "btAlignedObjectArray.h" + +const int BT_HASH_NULL=0xffffffff; + +template +class btHashKey +{ + int m_uid; +public: + + btHashKey(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKey getKey(const Value& value) const + { + return btHashKey(value.getUid()); + } +}; + + +template +class btHashKeyPtr +{ + int m_uid; +public: + + btHashKeyPtr(int uid) + :m_uid(uid) + { + } + + int getUid() const + { + return m_uid; + } + + //to our success + SIMD_FORCE_INLINE unsigned int getHash()const + { + int key = m_uid; + // Thomas Wang's hash + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return key; + } + + btHashKeyPtr getKey(const Value& value) const + { + return btHashKeyPtr(value->getUid()); + } +}; + +///The btHashMap template class implements a generic and lightweight hashmap. +///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp +template +class btHashMap +{ + + btAlignedObjectArray m_hashTable; + btAlignedObjectArray m_next; + btAlignedObjectArray m_valueArray; + + + + void growTables(const Key& key) + { + int newCapacity = m_valueArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_HASH_NULL; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_HASH_NULL; + } + + for(i=0;i= m_hashTable.size()) + { + return BT_HASH_NULL; + } + + int index = m_hashTable[hash]; + while ((index != BT_HASH_NULL) && (key.getUid() == key.getKey(m_valueArray[index]).getUid()) == false) + { + index = m_next[index]; + } + return index; + } + + void clear() + { + m_hashTable.clear(); + m_next.clear(); + m_valueArray.clear(); + } + +}; + +#endif //BT_HASH_MAP_H diff --git a/src/LinearMath/btMotionState.h b/src/LinearMath/btMotionState.h index c97922665..943181409 100644 --- a/src/LinearMath/btMotionState.h +++ b/src/LinearMath/btMotionState.h @@ -1,40 +1,40 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_MOTIONSTATE_H -#define BT_MOTIONSTATE_H - -#include "btTransform.h" - -///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics -///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) -class btMotionState -{ - public: - - virtual ~btMotionState() - { - - } - - virtual void getWorldTransform(btTransform& worldTrans ) const =0; - - //Bullet only calls the update of worldtransform for active objects - virtual void setWorldTransform(const btTransform& worldTrans)=0; - - -}; - -#endif //BT_MOTIONSTATE_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MOTIONSTATE_H +#define BT_MOTIONSTATE_H + +#include "btTransform.h" + +///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics +///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) +class btMotionState +{ + public: + + virtual ~btMotionState() + { + + } + + virtual void getWorldTransform(btTransform& worldTrans ) const =0; + + //Bullet only calls the update of worldtransform for active objects + virtual void setWorldTransform(const btTransform& worldTrans)=0; + + +}; + +#endif //BT_MOTIONSTATE_H diff --git a/src/LinearMath/btPoolAllocator.h b/src/LinearMath/btPoolAllocator.h index 8dcfbd6b0..39d2559c7 100755 --- a/src/LinearMath/btPoolAllocator.h +++ b/src/LinearMath/btPoolAllocator.h @@ -1,102 +1,102 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#ifndef _BT_POOL_ALLOCATOR_H -#define _BT_POOL_ALLOCATOR_H - -#include "btScalar.h" -#include "btAlignedAllocator.h" - -///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. -class btPoolAllocator -{ - int m_elemSize; - int m_maxElements; - int m_freeCount; - void* m_firstFree; - unsigned char* m_pool; - -public: - - btPoolAllocator(int elemSize, int maxElements) - :m_elemSize(elemSize), - m_maxElements(maxElements) - { - m_pool = (unsigned char*) btAlignedAlloc( static_cast(m_elemSize*m_maxElements),16); - - unsigned char* p = m_pool; - m_firstFree = p; - m_freeCount = m_maxElements; - int count = m_maxElements; - while (--count) { - *(void**)p = (p + m_elemSize); - p += m_elemSize; - } - *(void**)p = 0; - } - - ~btPoolAllocator() - { - btAlignedFree( m_pool); - } - - int getFreeCount() const - { - return m_freeCount; - } - - void* allocate(int size) - { - // release mode fix - (void)size; - btAssert(!size || size<=m_elemSize); - btAssert(m_freeCount>0); - void* result = m_firstFree; - m_firstFree = *(void**)m_firstFree; - --m_freeCount; - return result; - } - - bool validPtr(void* ptr) - { - if (ptr) { - if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) - { - return true; - } - } - return false; - } - - void freeMemory(void* ptr) - { - if (ptr) { - btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); - - *(void**)ptr = m_firstFree; - m_firstFree = ptr; - ++m_freeCount; - } - } - - int getElementSize() const - { - return m_elemSize; - } - - -}; - -#endif //_BT_POOL_ALLOCATOR_H +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef _BT_POOL_ALLOCATOR_H +#define _BT_POOL_ALLOCATOR_H + +#include "btScalar.h" +#include "btAlignedAllocator.h" + +///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. +class btPoolAllocator +{ + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + +public: + + btPoolAllocator(int elemSize, int maxElements) + :m_elemSize(elemSize), + m_maxElements(maxElements) + { + m_pool = (unsigned char*) btAlignedAlloc( static_cast(m_elemSize*m_maxElements),16); + + unsigned char* p = m_pool; + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } + + ~btPoolAllocator() + { + btAlignedFree( m_pool); + } + + int getFreeCount() const + { + return m_freeCount; + } + + void* allocate(int size) + { + // release mode fix + (void)size; + btAssert(!size || size<=m_elemSize); + btAssert(m_freeCount>0); + void* result = m_firstFree; + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + return result; + } + + bool validPtr(void* ptr) + { + if (ptr) { + if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) + { + return true; + } + } + return false; + } + + void freeMemory(void* ptr) + { + if (ptr) { + btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + } + } + + int getElementSize() const + { + return m_elemSize; + } + + +}; + +#endif //_BT_POOL_ALLOCATOR_H diff --git a/src/LinearMath/btStackAlloc.h b/src/LinearMath/btStackAlloc.h index bc5a79580..397b08487 100644 --- a/src/LinearMath/btStackAlloc.h +++ b/src/LinearMath/btStackAlloc.h @@ -1,116 +1,116 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* -StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson -Nov.2006 -*/ - -#ifndef BT_STACK_ALLOC -#define BT_STACK_ALLOC - -#include "btScalar.h" //for btAssert -#include "btAlignedAllocator.h" - -///The btBlock class is an internal structure for the btStackAlloc memory allocator. -struct btBlock -{ - btBlock* previous; - unsigned char* address; -}; - -///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out) -class btStackAlloc -{ -public: - - btStackAlloc(unsigned int size) { ctor();create(size); } - ~btStackAlloc() { destroy(); } - - inline void create(unsigned int size) - { - destroy(); - data = (unsigned char*) btAlignedAlloc(size,16); - totalsize = size; - } - inline void destroy() - { - btAssert(usedsize==0); - //Raise(L"StackAlloc is still in use"); - - if(usedsize==0) - { - if(!ischild && data) - btAlignedFree(data); - - data = 0; - usedsize = 0; - } - - } - - int getAvailableMemory() const - { - return static_cast(totalsize - usedsize); - } - - unsigned char* allocate(unsigned int size) - { - const unsigned int nus(usedsize+size); - if(nusprevious = current; - pb->address = data+usedsize; - current = pb; - return(pb); - } - SIMD_FORCE_INLINE void endBlock(btBlock* block) - { - btAssert(block==current); - //Raise(L"Unmatched blocks"); - if(block==current) - { - current = block->previous; - usedsize = (unsigned int)((block->address-data)-sizeof(btBlock)); - } - } - -private: - void ctor() - { - data = 0; - totalsize = 0; - usedsize = 0; - current = 0; - ischild = false; - } - unsigned char* data; - unsigned int totalsize; - unsigned int usedsize; - btBlock* current; - bool ischild; -}; - -#endif //BT_STACK_ALLOC +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson +Nov.2006 +*/ + +#ifndef BT_STACK_ALLOC +#define BT_STACK_ALLOC + +#include "btScalar.h" //for btAssert +#include "btAlignedAllocator.h" + +///The btBlock class is an internal structure for the btStackAlloc memory allocator. +struct btBlock +{ + btBlock* previous; + unsigned char* address; +}; + +///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out) +class btStackAlloc +{ +public: + + btStackAlloc(unsigned int size) { ctor();create(size); } + ~btStackAlloc() { destroy(); } + + inline void create(unsigned int size) + { + destroy(); + data = (unsigned char*) btAlignedAlloc(size,16); + totalsize = size; + } + inline void destroy() + { + btAssert(usedsize==0); + //Raise(L"StackAlloc is still in use"); + + if(usedsize==0) + { + if(!ischild && data) + btAlignedFree(data); + + data = 0; + usedsize = 0; + } + + } + + int getAvailableMemory() const + { + return static_cast(totalsize - usedsize); + } + + unsigned char* allocate(unsigned int size) + { + const unsigned int nus(usedsize+size); + if(nusprevious = current; + pb->address = data+usedsize; + current = pb; + return(pb); + } + SIMD_FORCE_INLINE void endBlock(btBlock* block) + { + btAssert(block==current); + //Raise(L"Unmatched blocks"); + if(block==current) + { + current = block->previous; + usedsize = (unsigned int)((block->address-data)-sizeof(btBlock)); + } + } + +private: + void ctor() + { + data = 0; + totalsize = 0; + usedsize = 0; + current = 0; + ischild = false; + } + unsigned char* data; + unsigned int totalsize; + unsigned int usedsize; + btBlock* current; + bool ischild; +}; + +#endif //BT_STACK_ALLOC diff --git a/src/LinearMath/ibmsdk/Makefile b/src/LinearMath/ibmsdk/Makefile index a80570aea..04148730f 100644 --- a/src/LinearMath/ibmsdk/Makefile +++ b/src/LinearMath/ibmsdk/Makefile @@ -1,39 +1,39 @@ -#### Source code Dirs -VPATH = ../ - -ROOT = ../../.. - -#### Library -LIBRARY_ppu = bulletmath.a - -#### Compiler flags -CPPFLAGS = \ --DUSE_LIBSPE2 \ --I$(ROOT)/src \ --I$(SDKINC) - -#### Optimization level flags -#CC_OPT_LEVEL = $(CC_OPT_LEVEL_DEBUG) -CC_OPT_LEVEL = -O3 - -##### Objects to be archived in lib - -OBJS = \ -btAlignedAllocator.o \ -btGeometryUtil.o \ -btQuickprof.o - -#### Install directories -INSTALL_DIR = $(ROOT)/lib/ibmsdk -INSTALL_FILES = $(LIBRARY_ppu) - -IBM_CELLSDK_VERSION := $(shell if [ -d /opt/cell ]; then echo "3.0"; fi) - -ifeq ("$(IBM_CELLSDK_VERSION)","3.0") - CELL_TOP ?= /opt/cell/sdk - include $(CELL_TOP)/buildutils/make.footer -else - CELL_TOP ?= /opt/ibm/cell-sdk/prototype - include $(CELL_TOP)/make.footer -endif - +#### Source code Dirs +VPATH = ../ + +ROOT = ../../.. + +#### Library +LIBRARY_ppu = bulletmath.a + +#### Compiler flags +CPPFLAGS = \ +-DUSE_LIBSPE2 \ +-I$(ROOT)/src \ +-I$(SDKINC) + +#### Optimization level flags +#CC_OPT_LEVEL = $(CC_OPT_LEVEL_DEBUG) +CC_OPT_LEVEL = -O3 + +##### Objects to be archived in lib + +OBJS = \ +btAlignedAllocator.o \ +btGeometryUtil.o \ +btQuickprof.o + +#### Install directories +INSTALL_DIR = $(ROOT)/lib/ibmsdk +INSTALL_FILES = $(LIBRARY_ppu) + +IBM_CELLSDK_VERSION := $(shell if [ -d /opt/cell ]; then echo "3.0"; fi) + +ifeq ("$(IBM_CELLSDK_VERSION)","3.0") + CELL_TOP ?= /opt/cell/sdk + include $(CELL_TOP)/buildutils/make.footer +else + CELL_TOP ?= /opt/ibm/cell-sdk/prototype + include $(CELL_TOP)/make.footer +endif + diff --git a/src/btBulletCollisionCommon.h b/src/btBulletCollisionCommon.h index 34f03c364..4b14f6d00 100644 --- a/src/btBulletCollisionCommon.h +++ b/src/btBulletCollisionCommon.h @@ -1,66 +1,66 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BULLET_COLLISION_COMMON_H -#define BULLET_COLLISION_COMMON_H - -///Common headerfile includes for Bullet Collision Detection - -///Bullet's btCollisionWorld and btCollisionObject definitions -#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" - -///Collision Shapes -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btCapsuleShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" -#include "BulletCollision/CollisionShapes/btConvexHullShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMesh.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" -#include "BulletCollision/CollisionShapes/btEmptyShape.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/CollisionShapes/btUniformScalingShape.h" - -///Narrowphase Collision Detector -#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" - -//btSphereBoxCollisionAlgorithm is broken, use gjk for now -//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" - -///Dispatching and generation of collision pairs (broadphase) -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" -#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h" -#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h" - -///Math library & Utils -#include "LinearMath/btQuaternion.h" -#include "LinearMath/btTransform.h" -#include "LinearMath/btDefaultMotionState.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btIDebugDraw.h" - -#endif //BULLET_COLLISION_COMMON_H - +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BULLET_COLLISION_COMMON_H +#define BULLET_COLLISION_COMMON_H + +///Common headerfile includes for Bullet Collision Detection + +///Bullet's btCollisionWorld and btCollisionObject definitions +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +///Collision Shapes +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMesh.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" +#include "BulletCollision/CollisionShapes/btEmptyShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btUniformScalingShape.h" + +///Narrowphase Collision Detector +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" + +//btSphereBoxCollisionAlgorithm is broken, use gjk for now +//#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" + +///Dispatching and generation of collision pairs (broadphase) +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" +#include "BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h" + +///Math library & Utils +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btDefaultMotionState.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btIDebugDraw.h" + +#endif //BULLET_COLLISION_COMMON_H +