Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

minor tweaks to demos: enable constraint debug drawing in AllBulletDemos, default constraint debugging size set to 0.3,

Browse Source 
set svn:eol-style native for folder files
http://code.google.com/p/bullet/issues/detail?id=191
This commit is contained in:
erwin.coumans
2009-02-18 22:52:03 +00:00
parent d9218378b0
commit 8acadeb711
126 changed files with 34617 additions and 34560 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
CMakeLists.txt
View File

@@ -1,71 +1,71 @@
cmake_minimum_required(VERSION 2.4) cmake_minimum_required(VERSION 2.4)
PROJECT(BULLET_PHYSICS) PROJECT(BULLET_PHYSICS)
SET(BULLET_VERSION 2.74) SET(BULLET_VERSION 2.74)
IF (NOT CMAKE_BUILD_TYPE) IF (NOT CMAKE_BUILD_TYPE)
# SET(CMAKE_BUILD_TYPE "Debug") # SET(CMAKE_BUILD_TYPE "Debug")
SET(CMAKE_BUILD_TYPE "Release") SET(CMAKE_BUILD_TYPE "Release")
ENDIF (NOT CMAKE_BUILD_TYPE) ENDIF (NOT CMAKE_BUILD_TYPE)
IF(COMMAND cmake_policy) IF(COMMAND cmake_policy)
cmake_policy(SET CMP0003 NEW) cmake_policy(SET CMP0003 NEW)
ENDIF(COMMAND cmake_policy) ENDIF(COMMAND cmake_policy)
# This is the shortcut to finding GLU, GLUT and OpenGL if they are properly installed on your system # This is the shortcut to finding GLU, GLUT and OpenGL if they are properly installed on your system
# This should be the case. # This should be the case.
FIND_PACKAGE(OpenGL) FIND_PACKAGE(OpenGL)
IF (OPENGL_FOUND) IF (OPENGL_FOUND)
MESSAGE("OPENGL FOUND") MESSAGE("OPENGL FOUND")
MESSAGE(${OPENGL_LIBRARIES}) MESSAGE(${OPENGL_LIBRARIES})
ELSE (OPENGL_FOUND) ELSE (OPENGL_FOUND)
MESSAGE("OPENGL NOT FOUND") MESSAGE("OPENGL NOT FOUND")
SET(OPENGL_gl_LIBRARY opengl32) SET(OPENGL_gl_LIBRARY opengl32)
SET(OPENGL_glu_LIBRARY glu32) SET(OPENGL_glu_LIBRARY glu32)
ENDIF (OPENGL_FOUND) ENDIF (OPENGL_FOUND)
# ADD_DEFINITIONS(-DBT_USE_FREEGLUT) # ADD_DEFINITIONS(-DBT_USE_FREEGLUT)
FIND_PACKAGE(GLU) FIND_PACKAGE(GLU)
FIND_PACKAGE(GLUT) FIND_PACKAGE(GLUT)
IF (GLUT_FOUND) IF (GLUT_FOUND)
MESSAGE("GLUT FOUND") MESSAGE("GLUT FOUND")
MESSAGE(${GLUT_glut_LIBRARY}) MESSAGE(${GLUT_glut_LIBRARY})
ELSE (GLUT_FOUND) ELSE (GLUT_FOUND)
IF (MINGW) IF (MINGW)
MESSAGE ("GLUT NOT FOUND not found, trying to use MINGW glut32") MESSAGE ("GLUT NOT FOUND not found, trying to use MINGW glut32")
SET(GLUT_glut_LIBRARY glut32) SET(GLUT_glut_LIBRARY glut32)
ENDIF (MINGW) ENDIF (MINGW)
IF (MSVC) IF (MSVC)
MESSAGE ("GLUT NOT FOUND, trying to use Bullet/Glut/glut32.lib for 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) SET(GLUT_glut_LIBRARY ${BULLET_PHYSICS_SOURCE_DIR}/Glut/glut32.lib)
ENDIF (MSVC) ENDIF (MSVC)
ENDIF (GLUT_FOUND) ENDIF (GLUT_FOUND)
IF (WIN32) IF (WIN32)
INCLUDE_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/Glut) INCLUDE_DIRECTORIES(${BULLET_PHYSICS_SOURCE_DIR}/Glut)
ELSE (WIN32) ELSE (WIN32)
# This is the lines for linux. This should always work if everything is installed and working fine. # 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}) INCLUDE_DIRECTORIES(/usr/include /usr/local/include ${GLUT_INCLUDE_DIR})
ENDIF (WIN32) ENDIF (WIN32)
OPTION(BUILD_DEMOS "Set when you want to build the demos" ON) OPTION(BUILD_DEMOS "Set when you want to build the demos" ON)
IF(BUILD_DEMOS) IF(BUILD_DEMOS)
SUBDIRS(Demos) SUBDIRS(Demos)
ENDIF(BUILD_DEMOS) ENDIF(BUILD_DEMOS)
OPTION(BUILD_EXTRAS "Set when you want to build the extras" ON) OPTION(BUILD_EXTRAS "Set when you want to build the extras" ON)
IF(BUILD_EXTRAS) IF(BUILD_EXTRAS)
SUBDIRS(Extras) SUBDIRS(Extras)
ENDIF(BUILD_EXTRAS) ENDIF(BUILD_EXTRAS)
SUBDIRS(src) SUBDIRS(src)

4
Demos/AllBulletDemos/DemoEntries.cpp
View File

@@ -103,13 +103,13 @@ public:
btDemoEntry g_demoEntries[] = btDemoEntry g_demoEntries[] =
{ {
{"ConstraintDemo",ConstraintDemo::Create},
{"ForkLift Demo",ForkLiftDemo::Create}, {"ForkLift Demo",ForkLiftDemo::Create},
{"Ragdoll Demo",RagdollDemo::Create}, {"Ragdoll Demo",RagdollDemo::Create},
{"Basic Demo", BasicDemo::Create}, {"Basic Demo", BasicDemo::Create},
{"Convex Decomposition",ConvexDecompositionDemo::Create}, {"Convex Decomposition",ConvexDecompositionDemo::Create},
{"Concave Moving", GimpactConcaveDemo::Create}, {"Concave Moving", GimpactConcaveDemo::Create},
{"Dynamic Control Demo",MotorDemo::Create}, // {"Dynamic Control Demo",MotorDemo::Create},
//{"ConcaveDemo",ConcaveDemo::Create}, //{"ConcaveDemo",ConcaveDemo::Create},
{"Concave Convexcast Demo",ConcaveConvexcastDemo::Create}, {"Concave Convexcast Demo",ConcaveConvexcastDemo::Create},
// {"SoftBody Cloth",SoftDemo0::Create}, // {"SoftBody Cloth",SoftDemo0::Create},

33
Demos/AllBulletDemos/Main.cpp
View File

@@ -52,6 +52,7 @@ namespace
int gDrawAabb; int gDrawAabb;
int gWireFrame; int gWireFrame;
int gHelpText; int gHelpText;
int gDebugConstraints;
int gDebugContacts; int gDebugContacts;
int gDrawTextures=1; int gDrawTextures=1;
int gDrawShadows=0; int gDrawShadows=0;
@@ -80,6 +81,14 @@ void setDefaultSettings()
gDrawAabb=0; gDrawAabb=0;
gWireFrame=0; gWireFrame=0;
gDebugContacts=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; gHelpText = 0;
gDrawTextures=1; gDrawTextures=1;
gDrawShadows=0; gDrawShadows=0;
@@ -118,6 +127,14 @@ void ResetScene()
void NextScene() void NextScene()
{ {
testSelection++; testSelection++;
if (testSelection>1)
{
gDebugConstraints=0;
} else
{
gDebugConstraints=1;
}
if(testSelection>23) if(testSelection>23)
testSelection=0; testSelection=0;
if (glui) if (glui)
@@ -216,6 +233,13 @@ void SimulationLoop()
{ {
demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_NoHelpText); 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) if (gDebugContacts)
{ {
demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawContactPoints); demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawContactPoints);
@@ -281,6 +305,14 @@ void SimulationLoop()
} }
if (testSelection != testIndex) if (testSelection != testIndex)
{ {
if (testSelection>1)
{
gDebugConstraints=0;
} else
{
gDebugConstraints=1;
}
testIndex = testSelection; testIndex = testSelection;
if (demo->getDynamicsWorld() && demo->getDynamicsWorld()->getDebugDrawer()) if (demo->getDynamicsWorld() && demo->getDynamicsWorld()->getDebugDrawer())
delete 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, "AABBs", &gDrawAabb);
glui->add_checkbox_to_panel(drawPanel, "Wireframe", &gWireFrame); glui->add_checkbox_to_panel(drawPanel, "Wireframe", &gWireFrame);
glui->add_checkbox_to_panel(drawPanel, "Contacts", &gDebugContacts); 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, "Textures", &gDrawTextures);
glui->add_checkbox_to_panel(drawPanel, "Shadows", &gDrawShadows); glui->add_checkbox_to_panel(drawPanel, "Shadows", &gDrawShadows);

2
Demos/ColladaDemo/ColladaDemo.cpp
View File

@@ -76,7 +76,7 @@ int main(int argc,char** argv)
/// Import Collada 1.4 Physics objects /// Import Collada 1.4 Physics objects
/// also can pass filename in as argument /// also can pass filename in as argument
const char* filename = "boxc4d.dae"; const char* filename = "jenga.dae";
printf("argc=%i\n",argc); printf("argc=%i\n",argc);
{ {
for (int i=0;i<argc;i++) for (int i=0;i<argc;i++)

22
Demos/RagdollDemo/RagdollDemo.cpp
View File

@@ -16,6 +16,8 @@ subject to the following restrictions:
Written by: Marten Svanfeldt Written by: Marten Svanfeldt
*/ */
#define CONSTRAINT_DEBUG_SIZE 0.2f
#include "btBulletDynamicsCommon.h" #include "btBulletDynamicsCommon.h"
#include "GlutStuff.h" #include "GlutStuff.h"
@@ -197,6 +199,8 @@ public:
hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
m_joints[JOINT_PELVIS_SPINE] = hingeC; m_joints[JOINT_PELVIS_SPINE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); 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 = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
m_joints[JOINT_SPINE_HEAD] = coneC; m_joints[JOINT_SPINE_HEAD] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); 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 = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0); coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_LEFT_HIP] = coneC; m_joints[JOINT_LEFT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
localA.setIdentity(); localB.setIdentity(); 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 = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_KNEE] = hingeC; m_joints[JOINT_LEFT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); 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 = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0); coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_RIGHT_HIP] = coneC; m_joints[JOINT_RIGHT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
localA.setIdentity(); localB.setIdentity(); 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 = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_KNEE] = hingeC; m_joints[JOINT_RIGHT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); 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.))); 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 = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0); coneC->setLimit(M_PI_2, M_PI_2, 0);
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[JOINT_LEFT_SHOULDER] = coneC; m_joints[JOINT_LEFT_SHOULDER] = coneC;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); 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(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_ELBOW] = hingeC; m_joints[JOINT_LEFT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); 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 = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0); coneC->setLimit(M_PI_2, M_PI_2, 0);
m_joints[JOINT_RIGHT_SHOULDER] = coneC; m_joints[JOINT_RIGHT_SHOULDER] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity(); localA.setIdentity(); localB.setIdentity();
@@ -277,6 +297,8 @@ public:
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); // hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_ELBOW] = hingeC; m_joints[JOINT_RIGHT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
} }

2590
src/BulletCollision/BroadphaseCollision/btDbvt.cpp
View File

File diff suppressed because it is too large Load Diff

2510
src/BulletCollision/BroadphaseCollision/btDbvt.h
View File

File diff suppressed because it is too large Load Diff

1454
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
View File

File diff suppressed because it is too large Load Diff

252
src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
View File

@@ -1,126 +1,126 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
///btDbvtBroadphase implementation by Nathanael Presson ///btDbvtBroadphase implementation by Nathanael Presson
#ifndef BT_DBVT_BROADPHASE_H #ifndef BT_DBVT_BROADPHASE_H
#define BT_DBVT_BROADPHASE_H #define BT_DBVT_BROADPHASE_H
#include "BulletCollision/BroadphaseCollision/btDbvt.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
// //
// Compile time config // Compile time config
// //
#define DBVT_BP_PROFILE 0 #define DBVT_BP_PROFILE 0
//#define DBVT_BP_SORTPAIRS 1 //#define DBVT_BP_SORTPAIRS 1
#define DBVT_BP_PREVENTFALSEUPDATE 0 #define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0 #define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0 #define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05 #define DBVT_BP_MARGIN (btScalar)0.05
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256 #define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
#endif #endif
// //
// btDbvtProxy // btDbvtProxy
// //
struct btDbvtProxy : btBroadphaseProxy struct btDbvtProxy : btBroadphaseProxy
{ {
/* Fields */ /* Fields */
//btDbvtAabbMm aabb; //btDbvtAabbMm aabb;
btDbvtNode* leaf; btDbvtNode* leaf;
btDbvtProxy* links[2]; btDbvtProxy* links[2];
int stage; int stage;
/* ctor */ /* ctor */
btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
{ {
links[0]=links[1]=0; links[0]=links[1]=0;
} }
}; };
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray; typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt). ///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. ///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. ///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 struct btDbvtBroadphase : btBroadphaseInterface
{ {
/* Config */ /* Config */
enum { enum {
DYNAMIC_SET = 0, /* Dynamic set index */ DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */ FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */ STAGECOUNT = 2 /* Number of stages */
}; };
/* Fields */ /* Fields */
btDbvt m_sets[2]; // Dbvt sets btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id int m_pid; // Parse id
int m_cid; // Cleanup index int m_cid; // Cleanup index
int m_gid; // Gen id int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup? bool m_needcleanup; // Need to run cleanup?
#if DBVT_BP_PROFILE #if DBVT_BP_PROFILE
btClock m_clock; btClock m_clock;
struct { struct {
unsigned long m_total; unsigned long m_total;
unsigned long m_ddcollide; unsigned long m_ddcollide;
unsigned long m_fdcollide; unsigned long m_fdcollide;
unsigned long m_cleanup; unsigned long m_cleanup;
unsigned long m_jobcount; unsigned long m_jobcount;
} m_profiling; } m_profiling;
#endif #endif
/* Methods */ /* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0); btDbvtBroadphase(btOverlappingPairCache* paircache=0);
~btDbvtBroadphase(); ~btDbvtBroadphase();
void collide(btDispatcher* dispatcher); void collide(btDispatcher* dispatcher);
void optimize(); void optimize();
/* btBroadphaseInterface Implementation */ /* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); 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 destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,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 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; virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
void calculateOverlappingPairs(btDispatcher* dispatcher); void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache(); btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const; const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats(); void printStats();
static void benchmark(btBroadphaseInterface*); static void benchmark(btBroadphaseInterface*);
void performDeferredRemoval(btDispatcher* dispatcher); void performDeferredRemoval(btDispatcher* dispatcher);
///reset broadphase internal structures, to ensure determinism/reproducability ///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher); virtual void resetPool(btDispatcher* dispatcher);
}; };
#endif #endif

978
src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
View File

@@ -1,489 +1,489 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btMultiSapBroadphase.h" #include "btMultiSapBroadphase.h"
#include "btSimpleBroadphase.h" #include "btSimpleBroadphase.h"
#include "LinearMath/btAabbUtil2.h" #include "LinearMath/btAabbUtil2.h"
#include "btQuantizedBvh.h" #include "btQuantizedBvh.h"
/// btSapBroadphaseArray m_sapBroadphases; /// btSapBroadphaseArray m_sapBroadphases;
/// btOverlappingPairCache* m_overlappingPairs; /// btOverlappingPairCache* m_overlappingPairs;
extern int gOverlappingPairs; extern int gOverlappingPairs;
/* /*
class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache class btMultiSapSortedOverlappingPairCache : public btSortedOverlappingPairCache
{ {
public: public:
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{ {
return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy); return btSortedOverlappingPairCache::addOverlappingPair((btBroadphaseProxy*)proxy0->m_multiSapParentProxy,(btBroadphaseProxy*)proxy1->m_multiSapParentProxy);
} }
}; };
*/ */
btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache) btMultiSapBroadphase::btMultiSapBroadphase(int /*maxProxies*/,btOverlappingPairCache* pairCache)
:m_overlappingPairs(pairCache), :m_overlappingPairs(pairCache),
m_optimizedAabbTree(0), m_optimizedAabbTree(0),
m_ownsPairCache(false), m_ownsPairCache(false),
m_invalidPair(0) m_invalidPair(0)
{ {
if (!m_overlappingPairs) if (!m_overlappingPairs)
{ {
m_ownsPairCache = true; m_ownsPairCache = true;
void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16); void* mem = btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16);
m_overlappingPairs = new (mem)btSortedOverlappingPairCache(); m_overlappingPairs = new (mem)btSortedOverlappingPairCache();
} }
struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback struct btMultiSapOverlapFilterCallback : public btOverlapFilterCallback
{ {
virtual ~btMultiSapOverlapFilterCallback() virtual ~btMultiSapOverlapFilterCallback()
{} {}
// return true when pairs need collision // return true when pairs need collision
virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const virtual bool needBroadphaseCollision(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) const
{ {
btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy; btBroadphaseProxy* multiProxy0 = (btBroadphaseProxy*)childProxy0->m_multiSapParentProxy;
btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy; btBroadphaseProxy* multiProxy1 = (btBroadphaseProxy*)childProxy1->m_multiSapParentProxy;
bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0; bool collides = (multiProxy0->m_collisionFilterGroup & multiProxy1->m_collisionFilterMask) != 0;
collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask); collides = collides && (multiProxy1->m_collisionFilterGroup & multiProxy0->m_collisionFilterMask);
return collides; return collides;
} }
}; };
void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16); void* mem = btAlignedAlloc(sizeof(btMultiSapOverlapFilterCallback),16);
m_filterCallback = new (mem)btMultiSapOverlapFilterCallback(); m_filterCallback = new (mem)btMultiSapOverlapFilterCallback();
m_overlappingPairs->setOverlapFilterCallback(m_filterCallback); m_overlappingPairs->setOverlapFilterCallback(m_filterCallback);
// mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16); // mem = btAlignedAlloc(sizeof(btSimpleBroadphase),16);
// m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs); // m_simpleBroadphase = new (mem) btSimpleBroadphase(maxProxies,m_overlappingPairs);
} }
btMultiSapBroadphase::~btMultiSapBroadphase() btMultiSapBroadphase::~btMultiSapBroadphase()
{ {
if (m_ownsPairCache) if (m_ownsPairCache)
{ {
m_overlappingPairs->~btOverlappingPairCache(); m_overlappingPairs->~btOverlappingPairCache();
btAlignedFree(m_overlappingPairs); btAlignedFree(m_overlappingPairs);
} }
} }
void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax) void btMultiSapBroadphase::buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax)
{ {
m_optimizedAabbTree = new btQuantizedBvh(); m_optimizedAabbTree = new btQuantizedBvh();
m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax); m_optimizedAabbTree->setQuantizationValues(bvhAabbMin,bvhAabbMax);
QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray(); QuantizedNodeArray& nodes = m_optimizedAabbTree->getLeafNodeArray();
for (int i=0;i<m_sapBroadphases.size();i++) for (int i=0;i<m_sapBroadphases.size();i++)
{ {
btQuantizedBvhNode node; btQuantizedBvhNode node;
btVector3 aabbMin,aabbMax; btVector3 aabbMin,aabbMax;
m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax); m_sapBroadphases[i]->getBroadphaseAabb(aabbMin,aabbMax);
m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0); m_optimizedAabbTree->quantize(&node.m_quantizedAabbMin[0],aabbMin,0);
m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1); m_optimizedAabbTree->quantize(&node.m_quantizedAabbMax[0],aabbMax,1);
int partId = 0; int partId = 0;
node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i; node.m_escapeIndexOrTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | i;
nodes.push_back(node); nodes.push_back(node);
} }
m_optimizedAabbTree->buildInternal(); 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*/) 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* ignoreMe -> we could think of recursive multi-sap, if someone is interested
void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16); void* mem = btAlignedAlloc(sizeof(btMultiSapProxy),16);
btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask); btMultiSapProxy* proxy = new (mem)btMultiSapProxy(aabbMin, aabbMax,shapeType,userPtr, collisionFilterGroup,collisionFilterMask);
m_multiSapProxies.push_back(proxy); m_multiSapProxies.push_back(proxy);
///this should deal with inserting/removal into child broadphases ///this should deal with inserting/removal into child broadphases
setAabb(proxy,aabbMin,aabbMax,dispatcher); setAabb(proxy,aabbMin,aabbMax,dispatcher);
return proxy; return proxy;
} }
void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
{ {
///not yet ///not yet
btAssert(0); btAssert(0);
} }
void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase) void btMultiSapBroadphase::addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase)
{ {
void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16); void* mem = btAlignedAlloc(sizeof(btBridgeProxy),16);
btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy; btBridgeProxy* bridgeProxyRef = new(mem) btBridgeProxy;
bridgeProxyRef->m_childProxy = childProxy; bridgeProxyRef->m_childProxy = childProxy;
bridgeProxyRef->m_childBroadphase = childBroadphase; bridgeProxyRef->m_childBroadphase = childBroadphase;
parentMultiSapProxy->m_bridgeProxies.push_back(bridgeProxyRef); 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);
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 return
amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() && amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() && amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ(); amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
} }
void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const void btMultiSapBroadphase::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const
{ {
btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
aabbMin = multiProxy->m_aabbMin; aabbMin = multiProxy->m_aabbMin;
aabbMax = multiProxy->m_aabbMax; aabbMax = multiProxy->m_aabbMax;
} }
void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax) void btMultiSapBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin,const btVector3& aabbMax)
{ {
for (int i=0;i<m_multiSapProxies.size();i++) for (int i=0;i<m_multiSapProxies.size();i++)
{ {
rayCallback.process(m_multiSapProxies[i]); rayCallback.process(m_multiSapProxies[i]);
} }
} }
//#include <stdio.h> //#include <stdio.h>
void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher) void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, btDispatcher* dispatcher)
{ {
btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy); btMultiSapProxy* multiProxy = static_cast<btMultiSapProxy*>(proxy);
multiProxy->m_aabbMin = aabbMin; multiProxy->m_aabbMin = aabbMin;
multiProxy->m_aabbMax = aabbMax; multiProxy->m_aabbMax = aabbMax;
// bool fullyContained = false; // bool fullyContained = false;
// bool alreadyInSimple = false; // bool alreadyInSimple = false;
struct MyNodeOverlapCallback : public btNodeOverlapCallback struct MyNodeOverlapCallback : public btNodeOverlapCallback
{ {
btMultiSapBroadphase* m_multiSap; btMultiSapBroadphase* m_multiSap;
btMultiSapProxy* m_multiProxy; btMultiSapProxy* m_multiProxy;
btDispatcher* m_dispatcher; btDispatcher* m_dispatcher;
MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher) MyNodeOverlapCallback(btMultiSapBroadphase* multiSap,btMultiSapProxy* multiProxy,btDispatcher* dispatcher)
:m_multiSap(multiSap), :m_multiSap(multiSap),
m_multiProxy(multiProxy), m_multiProxy(multiProxy),
m_dispatcher(dispatcher) m_dispatcher(dispatcher)
{ {
} }
virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex) virtual void processNode(int /*nodeSubPart*/, int broadphaseIndex)
{ {
btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex]; btBroadphaseInterface* childBroadphase = m_multiSap->getBroadphaseArray()[broadphaseIndex];
int containingBroadphaseIndex = -1; int containingBroadphaseIndex = -1;
//already found? //already found?
for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++) for (int i=0;i<m_multiProxy->m_bridgeProxies.size();i++)
{ {
if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) if (m_multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
{ {
containingBroadphaseIndex = i; containingBroadphaseIndex = i;
break; break;
} }
} }
if (containingBroadphaseIndex<0) if (containingBroadphaseIndex<0)
{ {
//add it //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); 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); m_multiSap->addToChildBroadphase(m_multiProxy,childProxy,childBroadphase);
} }
} }
}; };
MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher); MyNodeOverlapCallback myNodeCallback(this,multiProxy,dispatcher);
if (m_optimizedAabbTree) if (m_optimizedAabbTree)
m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); m_optimizedAabbTree->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
int i; int i;
for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
{ {
btVector3 worldAabbMin,worldAabbMax; btVector3 worldAabbMin,worldAabbMax;
multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); multiProxy->m_bridgeProxies[i]->m_childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
if (!overlapsBroadphase) if (!overlapsBroadphase)
{ {
//remove it now //remove it now
btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i]; btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[i];
btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1); multiProxy->m_bridgeProxies.swap( i,multiProxy->m_bridgeProxies.size()-1);
multiProxy->m_bridgeProxies.pop_back(); multiProxy->m_bridgeProxies.pop_back();
} }
} }
/* /*
if (1) if (1)
{ {
//find broadphase that contain this multiProxy //find broadphase that contain this multiProxy
int numChildBroadphases = getBroadphaseArray().size(); int numChildBroadphases = getBroadphaseArray().size();
for (int i=0;i<numChildBroadphases;i++) for (int i=0;i<numChildBroadphases;i++)
{ {
btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
btVector3 worldAabbMin,worldAabbMax; btVector3 worldAabbMin,worldAabbMax;
childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax); childBroadphase->getBroadphaseAabb(worldAabbMin,worldAabbMax);
bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); bool overlapsBroadphase = TestAabbAgainstAabb2(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
// fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax); // fullyContained = fullyContained || boxIsContainedWithinBox(worldAabbMin,worldAabbMax,multiProxy->m_aabbMin,multiProxy->m_aabbMax);
int containingBroadphaseIndex = -1; int containingBroadphaseIndex = -1;
//if already contains this //if already contains this
for (int i=0;i<multiProxy->m_bridgeProxies.size();i++) for (int i=0;i<multiProxy->m_bridgeProxies.size();i++)
{ {
if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase) if (multiProxy->m_bridgeProxies[i]->m_childBroadphase == childBroadphase)
{ {
containingBroadphaseIndex = i; containingBroadphaseIndex = i;
} }
alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase); alreadyInSimple = alreadyInSimple || (multiProxy->m_bridgeProxies[i]->m_childBroadphase == m_simpleBroadphase);
} }
if (overlapsBroadphase) if (overlapsBroadphase)
{ {
if (containingBroadphaseIndex<0) if (containingBroadphaseIndex<0)
{ {
btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher); btBroadphaseProxy* childProxy = childBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
childProxy->m_multiSapParentProxy = multiProxy; childProxy->m_multiSapParentProxy = multiProxy;
addToChildBroadphase(multiProxy,childProxy,childBroadphase); addToChildBroadphase(multiProxy,childProxy,childBroadphase);
} }
} else } else
{ {
if (containingBroadphaseIndex>=0) if (containingBroadphaseIndex>=0)
{ {
//remove //remove
btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex]; btBridgeProxy* bridgeProxy = multiProxy->m_bridgeProxies[containingBroadphaseIndex];
btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy; btBroadphaseProxy* childProxy = bridgeProxy->m_childProxy;
bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher); bridgeProxy->m_childBroadphase->destroyProxy(childProxy,dispatcher);
multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1); multiProxy->m_bridgeProxies.swap( containingBroadphaseIndex,multiProxy->m_bridgeProxies.size()-1);
multiProxy->m_bridgeProxies.pop_back(); multiProxy->m_bridgeProxies.pop_back();
} }
} }
} }
///If we are in no other child broadphase, stick the proxy in the global 'simple' broadphase (brute force) ///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) ///hopefully we don't end up with many entries here (can assert/provide feedback on stats)
if (0)//!multiProxy->m_bridgeProxies.size()) 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 ///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 ///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); btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
childProxy->m_multiSapParentProxy = multiProxy; childProxy->m_multiSapParentProxy = multiProxy;
addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
} }
} }
if (!multiProxy->m_bridgeProxies.size()) 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 ///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 ///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); btBroadphaseProxy* childProxy = m_simpleBroadphase->createProxy(aabbMin,aabbMax,multiProxy->m_shapeType,multiProxy->m_clientObject,multiProxy->m_collisionFilterGroup,multiProxy->m_collisionFilterMask, dispatcher);
childProxy->m_multiSapParentProxy = multiProxy; childProxy->m_multiSapParentProxy = multiProxy;
addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase); addToChildBroadphase(multiProxy,childProxy,m_simpleBroadphase);
} }
*/ */
//update //update
for ( i=0;i<multiProxy->m_bridgeProxies.size();i++) for ( i=0;i<multiProxy->m_bridgeProxies.size();i++)
{ {
btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i]; btBridgeProxy* bridgeProxyRef = multiProxy->m_bridgeProxies[i];
bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher); bridgeProxyRef->m_childBroadphase->setAabb(bridgeProxyRef->m_childProxy,aabbMin,aabbMax,dispatcher);
} }
} }
bool stopUpdating=false; bool stopUpdating=false;
class btMultiSapBroadphasePairSortPredicate class btMultiSapBroadphasePairSortPredicate
{ {
public: public:
bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 )
{ {
btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; 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* 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* 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; btMultiSapBroadphase::btMultiSapProxy* bProxy1 = b1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)b1.m_pProxy1->m_multiSapParentProxy : 0;
return aProxy0 > bProxy0 || return aProxy0 > bProxy0 ||
(aProxy0 == bProxy0 && aProxy1 > bProxy1) || (aProxy0 == bProxy0 && aProxy1 > bProxy1) ||
(aProxy0 == bProxy0 && aProxy1 == bProxy1 && a1.m_algorithm > b1.m_algorithm); (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 ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
{ {
// m_simpleBroadphase->calculateOverlappingPairs(dispatcher); // m_simpleBroadphase->calculateOverlappingPairs(dispatcher);
if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval()) if (!stopUpdating && getOverlappingPairCache()->hasDeferredRemoval())
{ {
btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray(); btBroadphasePairArray& overlappingPairArray = getOverlappingPairCache()->getOverlappingPairArray();
// quicksort(overlappingPairArray,0,overlappingPairArray.size()); // quicksort(overlappingPairArray,0,overlappingPairArray.size());
overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
// overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); // overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0; m_invalidPair = 0;
int i; int i;
btBroadphasePair previousPair; btBroadphasePair previousPair;
previousPair.m_pProxy0 = 0; previousPair.m_pProxy0 = 0;
previousPair.m_pProxy1 = 0; previousPair.m_pProxy1 = 0;
previousPair.m_algorithm = 0; previousPair.m_algorithm = 0;
for (i=0;i<overlappingPairArray.size();i++) for (i=0;i<overlappingPairArray.size();i++)
{ {
btBroadphasePair& pair = overlappingPairArray[i]; btBroadphasePair& pair = overlappingPairArray[i];
btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0; btMultiSapProxy* aProxy0 = pair.m_pProxy0 ? (btMultiSapProxy*)pair.m_pProxy0->m_multiSapParentProxy : 0;
btMultiSapProxy* aProxy1 = pair.m_pProxy1 ? (btMultiSapProxy*)pair.m_pProxy1->m_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* bProxy0 = previousPair.m_pProxy0 ? (btMultiSapProxy*)previousPair.m_pProxy0->m_multiSapParentProxy : 0;
btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0; btMultiSapProxy* bProxy1 = previousPair.m_pProxy1 ? (btMultiSapProxy*)previousPair.m_pProxy1->m_multiSapParentProxy : 0;
bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1); bool isDuplicate = (aProxy0 == bProxy0) && (aProxy1 == bProxy1);
previousPair = pair; previousPair = pair;
bool needsRemoval = false; bool needsRemoval = false;
if (!isDuplicate) if (!isDuplicate)
{ {
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
if (hasOverlap) if (hasOverlap)
{ {
needsRemoval = false;//callback->processOverlap(pair); needsRemoval = false;//callback->processOverlap(pair);
} else } else
{ {
needsRemoval = true; needsRemoval = true;
} }
} else } else
{ {
//remove duplicate //remove duplicate
needsRemoval = true; needsRemoval = true;
//should have no algorithm //should have no algorithm
btAssert(!pair.m_algorithm); btAssert(!pair.m_algorithm);
} }
if (needsRemoval) if (needsRemoval)
{ {
getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher); getOverlappingPairCache()->cleanOverlappingPair(pair,dispatcher);
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
// m_overlappingPairArray.pop_back(); // m_overlappingPairArray.pop_back();
pair.m_pProxy0 = 0; pair.m_pProxy0 = 0;
pair.m_pProxy1 = 0; pair.m_pProxy1 = 0;
m_invalidPair++; m_invalidPair++;
gOverlappingPairs--; gOverlappingPairs--;
} }
} }
///if you don't like to skip the invalid pairs in the array, execute following code: ///if you don't like to skip the invalid pairs in the array, execute following code:
#define CLEAN_INVALID_PAIRS 1 #define CLEAN_INVALID_PAIRS 1
#ifdef CLEAN_INVALID_PAIRS #ifdef CLEAN_INVALID_PAIRS
//perform a sort, to sort 'invalid' pairs to the end //perform a sort, to sort 'invalid' pairs to the end
//overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate()); //overlappingPairArray.heapSort(btMultiSapBroadphasePairSortPredicate());
overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate()); overlappingPairArray.quickSort(btMultiSapBroadphasePairSortPredicate());
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
m_invalidPair = 0; m_invalidPair = 0;
#endif//CLEAN_INVALID_PAIRS #endif//CLEAN_INVALID_PAIRS
//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
} }
} }
bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1) bool btMultiSapBroadphase::testAabbOverlap(btBroadphaseProxy* childProxy0,btBroadphaseProxy* childProxy1)
{ {
btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy; btMultiSapProxy* multiSapProxy0 = (btMultiSapProxy*)childProxy0->m_multiSapParentProxy;
btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy; btMultiSapProxy* multiSapProxy1 = (btMultiSapProxy*)childProxy1->m_multiSapParentProxy;
return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax, return TestAabbAgainstAabb2(multiSapProxy0->m_aabbMin,multiSapProxy0->m_aabbMax,
multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax); multiSapProxy1->m_aabbMin,multiSapProxy1->m_aabbMax);
} }
void btMultiSapBroadphase::printStats() void btMultiSapBroadphase::printStats()
{ {
/* printf("---------------------------------\n"); /* printf("---------------------------------\n");
printf("btMultiSapBroadphase.h\n"); printf("btMultiSapBroadphase.h\n");
printf("numHandles = %d\n",m_multiSapProxies.size()); printf("numHandles = %d\n",m_multiSapProxies.size());
//find broadphase that contain this multiProxy //find broadphase that contain this multiProxy
int numChildBroadphases = getBroadphaseArray().size(); int numChildBroadphases = getBroadphaseArray().size();
for (int i=0;i<numChildBroadphases;i++) for (int i=0;i<numChildBroadphases;i++)
{ {
btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i]; btBroadphaseInterface* childBroadphase = getBroadphaseArray()[i];
childBroadphase->printStats(); childBroadphase->printStats();
} }
*/ */
} }
void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher) void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
{ {
// not yet // not yet
} }

302
src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
View File

@@ -1,151 +1,151 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_MULTI_SAP_BROADPHASE #ifndef BT_MULTI_SAP_BROADPHASE
#define BT_MULTI_SAP_BROADPHASE #define BT_MULTI_SAP_BROADPHASE
#include "btBroadphaseInterface.h" #include "btBroadphaseInterface.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "btOverlappingPairCache.h" #include "btOverlappingPairCache.h"
class btBroadphaseInterface; class btBroadphaseInterface;
class btSimpleBroadphase; class btSimpleBroadphase;
typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray; typedef btAlignedObjectArray<btBroadphaseInterface*> btSapBroadphaseArray;
///The btMultiSapBroadphase is a research project, not recommended to use in production. Use btAxisSweep3 or btDbvtBroadphase instead. ///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 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. ///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. ///A btQuantizedBvh acceleration structures finds overlapping SAPs for each btBroadphaseProxy.
///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328 ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=328
///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329 ///and http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1329
class btMultiSapBroadphase :public btBroadphaseInterface class btMultiSapBroadphase :public btBroadphaseInterface
{ {
btSapBroadphaseArray m_sapBroadphases; btSapBroadphaseArray m_sapBroadphases;
btSimpleBroadphase* m_simpleBroadphase; btSimpleBroadphase* m_simpleBroadphase;
btOverlappingPairCache* m_overlappingPairs; btOverlappingPairCache* m_overlappingPairs;
class btQuantizedBvh* m_optimizedAabbTree; class btQuantizedBvh* m_optimizedAabbTree;
bool m_ownsPairCache; bool m_ownsPairCache;
btOverlapFilterCallback* m_filterCallback; btOverlapFilterCallback* m_filterCallback;
int m_invalidPair; int m_invalidPair;
struct btBridgeProxy struct btBridgeProxy
{ {
btBroadphaseProxy* m_childProxy; btBroadphaseProxy* m_childProxy;
btBroadphaseInterface* m_childBroadphase; btBroadphaseInterface* m_childBroadphase;
}; };
public: public:
struct btMultiSapProxy : public btBroadphaseProxy struct btMultiSapProxy : public btBroadphaseProxy
{ {
///array with all the entries that this proxy belongs to ///array with all the entries that this proxy belongs to
btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies; btAlignedObjectArray<btBridgeProxy*> m_bridgeProxies;
btVector3 m_aabbMin; btVector3 m_aabbMin;
btVector3 m_aabbMax; btVector3 m_aabbMax;
int m_shapeType; int m_shapeType;
/* void* m_userPtr; /* void* m_userPtr;
short int m_collisionFilterGroup; short int m_collisionFilterGroup;
short int m_collisionFilterMask; short int m_collisionFilterMask;
*/ */
btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) btMultiSapProxy(const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask)
:btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask), :btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask),
m_aabbMin(aabbMin), m_aabbMin(aabbMin),
m_aabbMax(aabbMax), m_aabbMax(aabbMax),
m_shapeType(shapeType) m_shapeType(shapeType)
{ {
m_multiSapParentProxy =this; m_multiSapParentProxy =this;
} }
}; };
protected: protected:
btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies; btAlignedObjectArray<btMultiSapProxy*> m_multiSapProxies;
public: public:
btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0); btMultiSapBroadphase(int maxProxies = 16384,btOverlappingPairCache* pairCache=0);
btSapBroadphaseArray& getBroadphaseArray() btSapBroadphaseArray& getBroadphaseArray()
{ {
return m_sapBroadphases; return m_sapBroadphases;
} }
const btSapBroadphaseArray& getBroadphaseArray() const const btSapBroadphaseArray& getBroadphaseArray() const
{ {
return m_sapBroadphases; return m_sapBroadphases;
} }
virtual ~btMultiSapBroadphase(); 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 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 destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax, 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 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)); 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); void addToChildBroadphase(btMultiSapProxy* parentMultiSapProxy, btBroadphaseProxy* childProxy, btBroadphaseInterface* childBroadphase);
///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
virtual void calculateOverlappingPairs(btDispatcher* dispatcher); virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
virtual btOverlappingPairCache* getOverlappingPairCache() virtual btOverlappingPairCache* getOverlappingPairCache()
{ {
return m_overlappingPairs; return m_overlappingPairs;
} }
virtual const btOverlappingPairCache* getOverlappingPairCache() const virtual const btOverlappingPairCache* getOverlappingPairCache() const
{ {
return m_overlappingPairs; return m_overlappingPairs;
} }
///getAabb returns the axis aligned bounding box in the 'global' coordinate frame ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame
///will add some transform later ///will add some transform later
virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
{ {
aabbMin.setValue(-1e30f,-1e30f,-1e30f); aabbMin.setValue(-1e30f,-1e30f,-1e30f);
aabbMax.setValue(1e30f,1e30f,1e30f); aabbMax.setValue(1e30f,1e30f,1e30f);
} }
void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax); void buildTree(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax);
virtual void printStats(); virtual void printStats();
void quicksort (btBroadphasePairArray& a, int lo, int hi); void quicksort (btBroadphasePairArray& a, int lo, int hi);
///reset broadphase internal structures, to ensure determinism/reproducability ///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher); virtual void resetPool(btDispatcher* dispatcher);
}; };
#endif //BT_MULTI_SAP_BROADPHASE #endif //BT_MULTI_SAP_BROADPHASE

1266
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
View File

File diff suppressed because it is too large Load Diff

936
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
View File

@@ -1,468 +1,468 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef OVERLAPPING_PAIR_CACHE_H #ifndef OVERLAPPING_PAIR_CACHE_H
#define OVERLAPPING_PAIR_CACHE_H #define OVERLAPPING_PAIR_CACHE_H
#include "btBroadphaseInterface.h" #include "btBroadphaseInterface.h"
#include "btBroadphaseProxy.h" #include "btBroadphaseProxy.h"
#include "btOverlappingPairCallback.h" #include "btOverlappingPairCallback.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
class btDispatcher; class btDispatcher;
typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray; typedef btAlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
struct btOverlapCallback struct btOverlapCallback
{ {
virtual ~btOverlapCallback() virtual ~btOverlapCallback()
{} {}
//return true for deletion of the pair //return true for deletion of the pair
virtual bool processOverlap(btBroadphasePair& pair) = 0; virtual bool processOverlap(btBroadphasePair& pair) = 0;
}; };
struct btOverlapFilterCallback struct btOverlapFilterCallback
{ {
virtual ~btOverlapFilterCallback() virtual ~btOverlapFilterCallback()
{} {}
// return true when pairs need collision // return true when pairs need collision
virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0;
}; };
extern int gRemovePairs; extern int gRemovePairs;
extern int gAddedPairs; extern int gAddedPairs;
extern int gFindPairs; extern int gFindPairs;
const int BT_NULL_PAIR=0xffffffff; const int BT_NULL_PAIR=0xffffffff;
///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. ///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. ///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
class btOverlappingPairCache : public btOverlappingPairCallback class btOverlappingPairCache : public btOverlappingPairCallback
{ {
public: public:
virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor virtual ~btOverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual btBroadphasePairArray& getOverlappingPairArray() = 0; virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
virtual int getNumOverlappingPairs() const = 0; virtual int getNumOverlappingPairs() const = 0;
virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0; virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) = 0;
virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0;
virtual bool hasDeferredRemoval() = 0; virtual bool hasDeferredRemoval() = 0;
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 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 /// 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 class btHashedOverlappingPairCache : public btOverlappingPairCache
{ {
btBroadphasePairArray m_overlappingPairArray; btBroadphasePairArray m_overlappingPairArray;
btOverlapFilterCallback* m_overlapFilterCallback; btOverlapFilterCallback* m_overlapFilterCallback;
bool m_blockedForChanges; bool m_blockedForChanges;
public: public:
btHashedOverlappingPairCache(); btHashedOverlappingPairCache();
virtual ~btHashedOverlappingPairCache(); virtual ~btHashedOverlappingPairCache();
void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const SIMD_FORCE_INLINE bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
{ {
if (m_overlapFilterCallback) if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides; return collides;
} }
// Add a pair and return the new pair. If the pair already exists, // Add a pair and return the new pair. If the pair already exists,
// no new pair is created and the old one is returned. // no new pair is created and the old one is returned.
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{ {
gAddedPairs++; gAddedPairs++;
if (!needsBroadphaseCollision(proxy0,proxy1)) if (!needsBroadphaseCollision(proxy0,proxy1))
return 0; return 0;
return internalAddPair(proxy0,proxy1); return internalAddPair(proxy0,proxy1);
} }
void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
virtual btBroadphasePair* getOverlappingPairArrayPtr() virtual btBroadphasePair* getOverlappingPairArrayPtr()
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
const btBroadphasePair* getOverlappingPairArrayPtr() const const btBroadphasePair* getOverlappingPairArrayPtr() const
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
btBroadphasePairArray& getOverlappingPairArray() btBroadphasePairArray& getOverlappingPairArray()
{ {
return m_overlappingPairArray; return m_overlappingPairArray;
} }
const btBroadphasePairArray& getOverlappingPairArray() const const btBroadphasePairArray& getOverlappingPairArray() const
{ {
return m_overlappingPairArray; return m_overlappingPairArray;
} }
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1); btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1);
int GetCount() const { return m_overlappingPairArray.size(); } int GetCount() const { return m_overlappingPairArray.size(); }
// btBroadphasePair* GetPairs() { return m_pairs; } // btBroadphasePair* GetPairs() { return m_pairs; }
btOverlapFilterCallback* getOverlapFilterCallback() btOverlapFilterCallback* getOverlapFilterCallback()
{ {
return m_overlapFilterCallback; return m_overlapFilterCallback;
} }
void setOverlapFilterCallback(btOverlapFilterCallback* callback) void setOverlapFilterCallback(btOverlapFilterCallback* callback)
{ {
m_overlapFilterCallback = callback; m_overlapFilterCallback = callback;
} }
int getNumOverlappingPairs() const int getNumOverlappingPairs() const
{ {
return m_overlappingPairArray.size(); return m_overlappingPairArray.size();
} }
private: private:
btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); btBroadphasePair* internalAddPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
void growTables(); void growTables();
SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
{ {
return pair.m_pProxy0->getUid() == proxyId1 && pair.m_pProxy1->getUid() == 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 // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
// This assumes proxyId1 and proxyId2 are 16-bit. // This assumes proxyId1 and proxyId2 are 16-bit.
SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2) SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
{ {
int key = (proxyId2 << 16) | proxyId1; int key = (proxyId2 << 16) | proxyId1;
key = ~key + (key << 15); key = ~key + (key << 15);
key = key ^ (key >> 12); key = key ^ (key >> 12);
key = key + (key << 2); key = key + (key << 2);
key = key ^ (key >> 4); key = key ^ (key >> 4);
key = key * 2057; key = key * 2057;
key = key ^ (key >> 16); key = key ^ (key >> 16);
return key; return key;
} }
*/ */
SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
{ {
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16));
// Thomas Wang's hash // Thomas Wang's hash
key += ~(key << 15); key += ~(key << 15);
key ^= (key >> 10); key ^= (key >> 10);
key += (key << 3); key += (key << 3);
key ^= (key >> 6); key ^= (key >> 6);
key += ~(key << 11); key += ~(key << 11);
key ^= (key >> 16); key ^= (key >> 16);
return static_cast<unsigned int>(key); return static_cast<unsigned int>(key);
} }
SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash) SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1, int hash)
{ {
int proxyId1 = proxy0->getUid(); int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid(); int proxyId2 = proxy1->getUid();
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
if (proxyId1 > proxyId2) if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2); btSwap(proxyId1, proxyId2);
#endif #endif
int index = m_hashTable[hash]; int index = m_hashTable[hash];
while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{ {
index = m_next[index]; index = m_next[index];
} }
if ( index == BT_NULL_PAIR ) if ( index == BT_NULL_PAIR )
{ {
return NULL; return NULL;
} }
btAssert(index < m_overlappingPairArray.size()); btAssert(index < m_overlappingPairArray.size());
return &m_overlappingPairArray[index]; return &m_overlappingPairArray[index];
} }
virtual bool hasDeferredRemoval() virtual bool hasDeferredRemoval()
{ {
return false; return false;
} }
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
{ {
m_ghostPairCallback = ghostPairCallback; m_ghostPairCallback = ghostPairCallback;
} }
virtual void sortOverlappingPairs(btDispatcher* dispatcher); virtual void sortOverlappingPairs(btDispatcher* dispatcher);
protected: protected:
btAlignedObjectArray<int> m_hashTable; btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next; btAlignedObjectArray<int> m_next;
btOverlappingPairCallback* m_ghostPairCallback; btOverlappingPairCallback* m_ghostPairCallback;
}; };
///btSortedOverlappingPairCache maintains the objects with overlapping AABB ///btSortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
class btSortedOverlappingPairCache : public btOverlappingPairCache class btSortedOverlappingPairCache : public btOverlappingPairCache
{ {
protected: protected:
//avoid brute-force finding all the time //avoid brute-force finding all the time
btBroadphasePairArray m_overlappingPairArray; btBroadphasePairArray m_overlappingPairArray;
//during the dispatch, check that user doesn't destroy/create proxy //during the dispatch, check that user doesn't destroy/create proxy
bool m_blockedForChanges; bool m_blockedForChanges;
///by default, do the removal during the pair traversal ///by default, do the removal during the pair traversal
bool m_hasDeferredRemoval; bool m_hasDeferredRemoval;
//if set, use the callback instead of the built in filter in needBroadphaseCollision //if set, use the callback instead of the built in filter in needBroadphaseCollision
btOverlapFilterCallback* m_overlapFilterCallback; btOverlapFilterCallback* m_overlapFilterCallback;
btOverlappingPairCallback* m_ghostPairCallback; btOverlappingPairCallback* m_ghostPairCallback;
public: public:
btSortedOverlappingPairCache(); btSortedOverlappingPairCache();
virtual ~btSortedOverlappingPairCache(); virtual ~btSortedOverlappingPairCache();
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher); void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher);
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const
{ {
if (m_overlapFilterCallback) if (m_overlapFilterCallback)
return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1);
bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides; return collides;
} }
btBroadphasePairArray& getOverlappingPairArray() btBroadphasePairArray& getOverlappingPairArray()
{ {
return m_overlappingPairArray; return m_overlappingPairArray;
} }
const btBroadphasePairArray& getOverlappingPairArray() const const btBroadphasePairArray& getOverlappingPairArray() const
{ {
return m_overlappingPairArray; return m_overlappingPairArray;
} }
btBroadphasePair* getOverlappingPairArrayPtr() btBroadphasePair* getOverlappingPairArrayPtr()
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
const btBroadphasePair* getOverlappingPairArrayPtr() const const btBroadphasePair* getOverlappingPairArrayPtr() const
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
int getNumOverlappingPairs() const int getNumOverlappingPairs() const
{ {
return m_overlappingPairArray.size(); return m_overlappingPairArray.size();
} }
btOverlapFilterCallback* getOverlapFilterCallback() btOverlapFilterCallback* getOverlapFilterCallback()
{ {
return m_overlapFilterCallback; return m_overlapFilterCallback;
} }
void setOverlapFilterCallback(btOverlapFilterCallback* callback) void setOverlapFilterCallback(btOverlapFilterCallback* callback)
{ {
m_overlapFilterCallback = callback; m_overlapFilterCallback = callback;
} }
virtual bool hasDeferredRemoval() virtual bool hasDeferredRemoval()
{ {
return m_hasDeferredRemoval; return m_hasDeferredRemoval;
} }
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
{ {
m_ghostPairCallback = ghostPairCallback; m_ghostPairCallback = ghostPairCallback;
} }
virtual void sortOverlappingPairs(btDispatcher* dispatcher); virtual void sortOverlappingPairs(btDispatcher* dispatcher);
}; };
///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. ///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class btNullPairCache : public btOverlappingPairCache class btNullPairCache : public btOverlappingPairCache
{ {
btBroadphasePairArray m_overlappingPairArray; btBroadphasePairArray m_overlappingPairArray;
public: public:
virtual btBroadphasePair* getOverlappingPairArrayPtr() virtual btBroadphasePair* getOverlappingPairArrayPtr()
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
const btBroadphasePair* getOverlappingPairArrayPtr() const const btBroadphasePair* getOverlappingPairArrayPtr() const
{ {
return &m_overlappingPairArray[0]; return &m_overlappingPairArray[0];
} }
btBroadphasePairArray& getOverlappingPairArray() btBroadphasePairArray& getOverlappingPairArray()
{ {
return m_overlappingPairArray; return m_overlappingPairArray;
} }
virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
{ {
} }
virtual int getNumOverlappingPairs() const virtual int getNumOverlappingPairs() const
{ {
return 0; return 0;
} }
virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/) virtual void cleanProxyFromPairs(btBroadphaseProxy* /*proxy*/,btDispatcher* /*dispatcher*/)
{ {
} }
virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
{ {
} }
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
{ {
} }
virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/) virtual btBroadphasePair* findPair(btBroadphaseProxy* /*proxy0*/, btBroadphaseProxy* /*proxy1*/)
{ {
return 0; return 0;
} }
virtual bool hasDeferredRemoval() virtual bool hasDeferredRemoval()
{ {
return true; return true;
} }
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
{ {
} }
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/) virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/)
{ {
return 0; return 0;
} }
virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/) virtual void* removeOverlappingPair(btBroadphaseProxy* /*proxy0*/,btBroadphaseProxy* /*proxy1*/,btDispatcher* /*dispatcher*/)
{ {
return 0; return 0;
} }
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/) virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* /*proxy0*/,btDispatcher* /*dispatcher*/)
{ {
} }
virtual void sortOverlappingPairs(btDispatcher* dispatcher) virtual void sortOverlappingPairs(btDispatcher* dispatcher)
{ {
} }
}; };
#endif //OVERLAPPING_PAIR_CACHE_H #endif //OVERLAPPING_PAIR_CACHE_H

2296
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
View File

File diff suppressed because it is too large Load Diff

946
src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h
View File

@@ -1,473 +1,473 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef QUANTIZED_BVH_H #ifndef QUANTIZED_BVH_H
#define QUANTIZED_BVH_H #define QUANTIZED_BVH_H
//#define DEBUG_CHECK_DEQUANTIZATION 1 //#define DEBUG_CHECK_DEQUANTIZATION 1
#ifdef DEBUG_CHECK_DEQUANTIZATION #ifdef DEBUG_CHECK_DEQUANTIZATION
#ifdef __SPU__ #ifdef __SPU__
#define printf spu_printf #define printf spu_printf
#endif //__SPU__ #endif //__SPU__
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#endif //DEBUG_CHECK_DEQUANTIZATION #endif //DEBUG_CHECK_DEQUANTIZATION
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp //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 //Note: currently we have 16 bytes per quantized node
#define MAX_SUBTREE_SIZE_IN_BYTES 2048 #define MAX_SUBTREE_SIZE_IN_BYTES 2048
// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
// actually) triangles each (since the sign bit is reserved // actually) triangles each (since the sign bit is reserved
#define MAX_NUM_PARTS_IN_BITS 10 #define MAX_NUM_PARTS_IN_BITS 10
///btQuantizedBvhNode is a compressed aabb node, 16 bytes. ///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). ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
//12 bytes //12 bytes
unsigned short int m_quantizedAabbMin[3]; unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3]; unsigned short int m_quantizedAabbMax[3];
//4 bytes //4 bytes
int m_escapeIndexOrTriangleIndex; int m_escapeIndexOrTriangleIndex;
bool isLeafNode() const bool isLeafNode() const
{ {
//skipindex is negative (internal node), triangleindex >=0 (leafnode) //skipindex is negative (internal node), triangleindex >=0 (leafnode)
return (m_escapeIndexOrTriangleIndex >= 0); return (m_escapeIndexOrTriangleIndex >= 0);
} }
int getEscapeIndex() const int getEscapeIndex() const
{ {
btAssert(!isLeafNode()); btAssert(!isLeafNode());
return -m_escapeIndexOrTriangleIndex; return -m_escapeIndexOrTriangleIndex;
} }
int getTriangleIndex() const int getTriangleIndex() const
{ {
btAssert(isLeafNode()); btAssert(isLeafNode());
// Get only the lower bits where the triangle index is stored // Get only the lower bits where the triangle index is stored
return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
} }
int getPartId() const int getPartId() const
{ {
btAssert(isLeafNode()); btAssert(isLeafNode());
// Get only the highest bits where the part index is stored // Get only the highest bits where the part index is stored
return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
} }
} }
; ;
/// btOptimizedBvhNode contains both internal and leaf node information. /// btOptimizedBvhNode contains both internal and leaf node information.
/// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. /// Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
//32 bytes //32 bytes
btVector3 m_aabbMinOrg; btVector3 m_aabbMinOrg;
btVector3 m_aabbMaxOrg; btVector3 m_aabbMaxOrg;
//4 //4
int m_escapeIndex; int m_escapeIndex;
//8 //8
//for child nodes //for child nodes
int m_subPart; int m_subPart;
int m_triangleIndex; int m_triangleIndex;
int m_padding[5];//bad, due to alignment int m_padding[5];//bad, due to alignment
}; };
///btBvhSubtreeInfo provides info to gather a subtree of limited size ///btBvhSubtreeInfo provides info to gather a subtree of limited size
ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo ATTRIBUTE_ALIGNED16(class) btBvhSubtreeInfo
{ {
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
//12 bytes //12 bytes
unsigned short int m_quantizedAabbMin[3]; unsigned short int m_quantizedAabbMin[3];
unsigned short int m_quantizedAabbMax[3]; unsigned short int m_quantizedAabbMax[3];
//4 bytes, points to the root of the subtree //4 bytes, points to the root of the subtree
int m_rootNodeIndex; int m_rootNodeIndex;
//4 bytes //4 bytes
int m_subtreeSize; int m_subtreeSize;
int m_padding[3]; int m_padding[3];
btBvhSubtreeInfo() btBvhSubtreeInfo()
{ {
//memset(&m_padding[0], 0, sizeof(m_padding)); //memset(&m_padding[0], 0, sizeof(m_padding));
} }
void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode) void setAabbFromQuantizeNode(const btQuantizedBvhNode& quantizedNode)
{ {
m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0]; m_quantizedAabbMin[0] = quantizedNode.m_quantizedAabbMin[0];
m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1]; m_quantizedAabbMin[1] = quantizedNode.m_quantizedAabbMin[1];
m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2]; m_quantizedAabbMin[2] = quantizedNode.m_quantizedAabbMin[2];
m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0]; m_quantizedAabbMax[0] = quantizedNode.m_quantizedAabbMax[0];
m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1]; m_quantizedAabbMax[1] = quantizedNode.m_quantizedAabbMax[1];
m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2]; m_quantizedAabbMax[2] = quantizedNode.m_quantizedAabbMax[2];
} }
} }
; ;
class btNodeOverlapCallback class btNodeOverlapCallback
{ {
public: public:
virtual ~btNodeOverlapCallback() {}; virtual ~btNodeOverlapCallback() {};
virtual void processNode(int subPart, int triangleIndex) = 0; virtual void processNode(int subPart, int triangleIndex) = 0;
}; };
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
///for code readability: ///for code readability:
typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray; typedef btAlignedObjectArray<btOptimizedBvhNode> NodeArray;
typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray; typedef btAlignedObjectArray<btQuantizedBvhNode> QuantizedNodeArray;
typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray; typedef btAlignedObjectArray<btBvhSubtreeInfo> BvhSubtreeInfoArray;
///The btQuantizedBvh class stores an AABB tree that can be quickly traversed on CPU and Cell SPU. ///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 used by the btBvhTriangleMeshShape as midphase, and by the btMultiSapBroadphase.
///It is recommended to use quantization for better performance and lower memory requirements. ///It is recommended to use quantization for better performance and lower memory requirements.
ATTRIBUTE_ALIGNED16(class) btQuantizedBvh ATTRIBUTE_ALIGNED16(class) btQuantizedBvh
{ {
public: public:
enum btTraversalMode enum btTraversalMode
{ {
TRAVERSAL_STACKLESS = 0, TRAVERSAL_STACKLESS = 0,
TRAVERSAL_STACKLESS_CACHE_FRIENDLY, TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
TRAVERSAL_RECURSIVE TRAVERSAL_RECURSIVE
}; };
protected: protected:
btVector3 m_bvhAabbMin; btVector3 m_bvhAabbMin;
btVector3 m_bvhAabbMax; btVector3 m_bvhAabbMax;
btVector3 m_bvhQuantization; btVector3 m_bvhQuantization;
int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess. int m_bulletVersion; //for serialization versioning. It could also be used to detect endianess.
int m_curNodeIndex; int m_curNodeIndex;
//quantization data //quantization data
bool m_useQuantization; bool m_useQuantization;
NodeArray m_leafNodes; NodeArray m_leafNodes;
NodeArray m_contiguousNodes; NodeArray m_contiguousNodes;
QuantizedNodeArray m_quantizedLeafNodes; QuantizedNodeArray m_quantizedLeafNodes;
QuantizedNodeArray m_quantizedContiguousNodes; QuantizedNodeArray m_quantizedContiguousNodes;
btTraversalMode m_traversalMode; btTraversalMode m_traversalMode;
BvhSubtreeInfoArray m_SubtreeHeaders; BvhSubtreeInfoArray m_SubtreeHeaders;
//This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray //This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
int m_subtreeHeaderCount; int m_subtreeHeaderCount;
///two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) ///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 ///this might be refactored into a virtual, it is usually not calculated at run-time
void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin) void setInternalNodeAabbMin(int nodeIndex, const btVector3& aabbMin)
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0); quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[0] ,aabbMin,0);
} else } else
{ {
m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin; m_contiguousNodes[nodeIndex].m_aabbMinOrg = aabbMin;
} }
} }
void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax) void setInternalNodeAabbMax(int nodeIndex,const btVector3& aabbMax)
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1); quantize(&m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[0],aabbMax,1);
} else } else
{ {
m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax; m_contiguousNodes[nodeIndex].m_aabbMaxOrg = aabbMax;
} }
} }
btVector3 getAabbMin(int nodeIndex) const btVector3 getAabbMin(int nodeIndex) const
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]); return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMin[0]);
} }
//non-quantized //non-quantized
return m_leafNodes[nodeIndex].m_aabbMinOrg; return m_leafNodes[nodeIndex].m_aabbMinOrg;
} }
btVector3 getAabbMax(int nodeIndex) const btVector3 getAabbMax(int nodeIndex) const
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]); return unQuantize(&m_quantizedLeafNodes[nodeIndex].m_quantizedAabbMax[0]);
} }
//non-quantized //non-quantized
return m_leafNodes[nodeIndex].m_aabbMaxOrg; return m_leafNodes[nodeIndex].m_aabbMaxOrg;
} }
void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex)
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex; m_quantizedContiguousNodes[nodeIndex].m_escapeIndexOrTriangleIndex = -escapeIndex;
} }
else else
{ {
m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex; m_contiguousNodes[nodeIndex].m_escapeIndex = escapeIndex;
} }
} }
void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax) void mergeInternalNodeAabb(int nodeIndex,const btVector3& newAabbMin,const btVector3& newAabbMax)
{ {
if (m_useQuantization) if (m_useQuantization)
{ {
unsigned short int quantizedAabbMin[3]; unsigned short int quantizedAabbMin[3];
unsigned short int quantizedAabbMax[3]; unsigned short int quantizedAabbMax[3];
quantize(quantizedAabbMin,newAabbMin,0); quantize(quantizedAabbMin,newAabbMin,0);
quantize(quantizedAabbMax,newAabbMax,1); quantize(quantizedAabbMax,newAabbMax,1);
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i]) if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMin[i] > quantizedAabbMin[i])
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]) if (m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] < quantizedAabbMax[i])
m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i]; m_quantizedContiguousNodes[nodeIndex].m_quantizedAabbMax[i] = quantizedAabbMax[i];
} }
} else } else
{ {
//non-quantized //non-quantized
m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin); m_contiguousNodes[nodeIndex].m_aabbMinOrg.setMin(newAabbMin);
m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax); m_contiguousNodes[nodeIndex].m_aabbMaxOrg.setMax(newAabbMax);
} }
} }
void swapLeafNodes(int firstIndex,int secondIndex); void swapLeafNodes(int firstIndex,int secondIndex);
void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex); void assignInternalNodeFromLeafNode(int internalNode,int leafNodeIndex);
protected: protected:
void buildTree (int startIndex,int endIndex); void buildTree (int startIndex,int endIndex);
int calcSplittingAxis(int startIndex,int endIndex); int calcSplittingAxis(int startIndex,int endIndex);
int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis); int sortAndCalcSplittingIndex(int startIndex,int endIndex,int splitAxis);
void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; 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 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 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; 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 ///tree traversal designed for small-memory processors like PS3 SPU
void walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax) const; 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 ///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; 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 ///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 walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const;
void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex);
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btQuantizedBvh(); btQuantizedBvh();
virtual ~btQuantizedBvh(); virtual ~btQuantizedBvh();
///***************************************** expert/internal use only ************************* ///***************************************** expert/internal use only *************************
void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0));
QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; }
///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized
void buildInternal(); void buildInternal();
///***************************************** expert/internal use only ************************* ///***************************************** expert/internal use only *************************
void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const;
void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) 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; 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 SIMD_FORCE_INLINE void quantize(unsigned short* out, const btVector3& point,int isMax) const
{ {
btAssert(m_useQuantization); btAssert(m_useQuantization);
btAssert(point.getX() <= m_bvhAabbMax.getX()); btAssert(point.getX() <= m_bvhAabbMax.getX());
btAssert(point.getY() <= m_bvhAabbMax.getY()); btAssert(point.getY() <= m_bvhAabbMax.getY());
btAssert(point.getZ() <= m_bvhAabbMax.getZ()); btAssert(point.getZ() <= m_bvhAabbMax.getZ());
btAssert(point.getX() >= m_bvhAabbMin.getX()); btAssert(point.getX() >= m_bvhAabbMin.getX());
btAssert(point.getY() >= m_bvhAabbMin.getY()); btAssert(point.getY() >= m_bvhAabbMin.getY());
btAssert(point.getZ() >= m_bvhAabbMin.getZ()); btAssert(point.getZ() >= m_bvhAabbMin.getZ());
btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization; btVector3 v = (point - m_bvhAabbMin) * m_bvhQuantization;
///Make sure rounding is done in a way that unQuantize(quantizeWithClamp(...)) is conservative ///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) ///end-points always set the first bit, so that they are sorted properly (so that neighbouring AABBs overlap properly)
///@todo: double-check this ///@todo: double-check this
if (isMax) if (isMax)
{ {
out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1)); out[0] = (unsigned short) (((unsigned short)(v.getX()+btScalar(1.)) | 1));
out[1] = (unsigned short) (((unsigned short)(v.getY()+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)); out[2] = (unsigned short) (((unsigned short)(v.getZ()+btScalar(1.)) | 1));
} else } else
{ {
out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe)); out[0] = (unsigned short) (((unsigned short)(v.getX()) & 0xfffe));
out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe)); out[1] = (unsigned short) (((unsigned short)(v.getY()) & 0xfffe));
out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe)); out[2] = (unsigned short) (((unsigned short)(v.getZ()) & 0xfffe));
} }
#ifdef DEBUG_CHECK_DEQUANTIZATION #ifdef DEBUG_CHECK_DEQUANTIZATION
btVector3 newPoint = unQuantize(out); btVector3 newPoint = unQuantize(out);
if (isMax) if (isMax)
{ {
if (newPoint.getX() < point.getX()) if (newPoint.getX() < point.getX())
{ {
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), 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()) if (newPoint.getY() < point.getY())
{ {
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), 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()) if (newPoint.getZ() < point.getZ())
{ {
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ()); printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), newPoint.getZ(),point.getZ());
} }
} else } else
{ {
if (newPoint.getX() > point.getX()) if (newPoint.getX() > point.getX())
{ {
printf("unconservative X, diffX = %f, oldX=%f,newX=%f\n",newPoint.getX()-point.getX(), 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()) if (newPoint.getY() > point.getY())
{ {
printf("unconservative Y, diffY = %f, oldY=%f,newY=%f\n",newPoint.getY()-point.getY(), 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()) if (newPoint.getZ() > point.getZ())
{ {
printf("unconservative Z, diffZ = %f, oldZ=%f,newZ=%f\n",newPoint.getZ()-point.getZ(), 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 #endif //DEBUG_CHECK_DEQUANTIZATION
} }
SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point2,int isMax) const
{ {
btAssert(m_useQuantization); btAssert(m_useQuantization);
btVector3 clampedPoint(point2); btVector3 clampedPoint(point2);
clampedPoint.setMax(m_bvhAabbMin); clampedPoint.setMax(m_bvhAabbMin);
clampedPoint.setMin(m_bvhAabbMax); clampedPoint.setMin(m_bvhAabbMax);
quantize(out,clampedPoint,isMax); quantize(out,clampedPoint,isMax);
} }
SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const SIMD_FORCE_INLINE btVector3 unQuantize(const unsigned short* vecIn) const
{ {
btVector3 vecOut; btVector3 vecOut;
vecOut.setValue( vecOut.setValue(
(btScalar)(vecIn[0]) / (m_bvhQuantization.getX()), (btScalar)(vecIn[0]) / (m_bvhQuantization.getX()),
(btScalar)(vecIn[1]) / (m_bvhQuantization.getY()), (btScalar)(vecIn[1]) / (m_bvhQuantization.getY()),
(btScalar)(vecIn[2]) / (m_bvhQuantization.getZ())); (btScalar)(vecIn[2]) / (m_bvhQuantization.getZ()));
vecOut += m_bvhAabbMin; vecOut += m_bvhAabbMin;
return vecOut; return vecOut;
} }
///setTraversalMode let's you choose between stackless, recursive or stackless cache friendly tree traversal. Note this is only implemented for quantized trees. ///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) void setTraversalMode(btTraversalMode traversalMode)
{ {
m_traversalMode = traversalMode; m_traversalMode = traversalMode;
} }
SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray() SIMD_FORCE_INLINE QuantizedNodeArray& getQuantizedNodeArray()
{ {
return m_quantizedContiguousNodes; return m_quantizedContiguousNodes;
} }
SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray() SIMD_FORCE_INLINE BvhSubtreeInfoArray& getSubtreeInfoArray()
{ {
return m_SubtreeHeaders; return m_SubtreeHeaders;
} }
/////Calculate space needed to store BVH for serialization /////Calculate space needed to store BVH for serialization
unsigned calculateSerializeBufferSize(); unsigned calculateSerializeBufferSize();
/// Data buffer MUST be 16 byte aligned /// Data buffer MUST be 16 byte aligned
virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian); 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' ///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 btQuantizedBvh *deSerializeInPlace(void *i_alignedDataBuffer, unsigned int i_dataBufferSize, bool i_swapEndian);
static unsigned int getAlignmentSerializationPadding(); static unsigned int getAlignmentSerializationPadding();
SIMD_FORCE_INLINE bool isQuantized() SIMD_FORCE_INLINE bool isQuantized()
{ {
return m_useQuantization; return m_useQuantization;
} }
private: private:
// Special "copy" constructor that allows for in-place deserialization // Special "copy" constructor that allows for in-place deserialization
// Prevents btVector3's default constructor from being called, but doesn't inialize much else // 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) // 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); btQuantizedBvh(btQuantizedBvh &other, bool ownsMemory);
} }
; ;
#endif //QUANTIZED_BVH_H #endif //QUANTIZED_BVH_H

418
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -1,209 +1,209 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "SphereTriangleDetector.h" #include "SphereTriangleDetector.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
:m_sphere(sphere), :m_sphere(sphere),
m_triangle(triangle), m_triangle(triangle),
m_contactBreakingThreshold(contactBreakingThreshold) m_contactBreakingThreshold(contactBreakingThreshold)
{ {
} }
void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
{ {
(void)debugDraw; (void)debugDraw;
const btTransform& transformA = input.m_transformA; const btTransform& transformA = input.m_transformA;
const btTransform& transformB = input.m_transformB; const btTransform& transformB = input.m_transformB;
btVector3 point,normal; btVector3 point,normal;
btScalar timeOfImpact = btScalar(1.); btScalar timeOfImpact = btScalar(1.);
btScalar depth = btScalar(0.); btScalar depth = btScalar(0.);
// output.m_distance = btScalar(1e30); // output.m_distance = btScalar(1e30);
//move sphere into triangle space //move sphere into triangle space
btTransform sphereInTr = transformB.inverseTimes(transformA); btTransform sphereInTr = transformB.inverseTimes(transformA);
if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
{ {
if (swapResults) if (swapResults)
{ {
btVector3 normalOnB = transformB.getBasis()*normal; btVector3 normalOnB = transformB.getBasis()*normal;
btVector3 normalOnA = -normalOnB; btVector3 normalOnA = -normalOnB;
btVector3 pointOnA = transformB*point+normalOnB*depth; btVector3 pointOnA = transformB*point+normalOnB*depth;
output.addContactPoint(normalOnA,pointOnA,depth); output.addContactPoint(normalOnA,pointOnA,depth);
} else } else
{ {
output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
} }
} }
} }
#define MAX_OVERLAP btScalar(0.) #define MAX_OVERLAP btScalar(0.)
// See also geometrictools.com // See also geometrictools.com
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv // 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);
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 diff = p - from;
btVector3 v = to - from; btVector3 v = to - from;
btScalar t = v.dot(diff); btScalar t = v.dot(diff);
if (t > 0) { if (t > 0) {
btScalar dotVV = v.dot(v); btScalar dotVV = v.dot(v);
if (t < dotVV) { if (t < dotVV) {
t /= dotVV; t /= dotVV;
diff -= t*v; diff -= t*v;
} else { } else {
t = 1; t = 1;
diff -= v; diff -= v;
} }
} else } else
t = 0; t = 0;
nearest = from + t*v; nearest = from + t*v;
return diff.dot(diff); return diff.dot(diff);
} }
bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
btVector3 lp(p); btVector3 lp(p);
btVector3 lnormal(normal); btVector3 lnormal(normal);
return pointInTriangle(vertices, lnormal, &lp); return pointInTriangle(vertices, lnormal, &lp);
} }
///combined discrete/continuous sphere-triangle ///combined discrete/continuous sphere-triangle
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) 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* vertices = &m_triangle->getVertexPtr(0);
const btVector3& c = sphereCenter; const btVector3& c = sphereCenter;
btScalar r = m_sphere->getRadius(); btScalar r = m_sphere->getRadius();
btVector3 delta (0,0,0); btVector3 delta (0,0,0);
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize(); normal.normalize();
btVector3 p1ToCentre = c - vertices[0]; btVector3 p1ToCentre = c - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal); btScalar distanceFromPlane = p1ToCentre.dot(normal);
if (distanceFromPlane < btScalar(0.)) if (distanceFromPlane < btScalar(0.))
{ {
//triangle facing the other way //triangle facing the other way
distanceFromPlane *= btScalar(-1.); distanceFromPlane *= btScalar(-1.);
normal *= btScalar(-1.); normal *= btScalar(-1.);
} }
btScalar contactMargin = contactBreakingThreshold; btScalar contactMargin = contactBreakingThreshold;
bool isInsideContactPlane = distanceFromPlane < r + contactMargin; bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
bool isInsideShellPlane = distanceFromPlane < r; bool isInsideShellPlane = distanceFromPlane < r;
btScalar deltaDotNormal = delta.dot(normal); btScalar deltaDotNormal = delta.dot(normal);
if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0)) if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
return false; return false;
// Check for contact / intersection // Check for contact / intersection
bool hasContact = false; bool hasContact = false;
btVector3 contactPoint; btVector3 contactPoint;
if (isInsideContactPlane) { if (isInsideContactPlane) {
if (facecontains(c,vertices,normal)) { if (facecontains(c,vertices,normal)) {
// Inside the contact wedge - touches a point on the shell plane // Inside the contact wedge - touches a point on the shell plane
hasContact = true; hasContact = true;
contactPoint = c - normal*distanceFromPlane; contactPoint = c - normal*distanceFromPlane;
} else { } else {
// Could be inside one of the contact capsules // Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
btVector3 nearestOnEdge; btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) { for (int i = 0; i < m_triangle->getNumEdges(); i++) {
btVector3 pa; btVector3 pa;
btVector3 pb; btVector3 pb;
m_triangle->getEdge(i,pa,pb); m_triangle->getEdge(i,pa,pb);
btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr) { if (distanceSqr < contactCapsuleRadiusSqr) {
// Yep, we're inside a capsule // Yep, we're inside a capsule
hasContact = true; hasContact = true;
contactPoint = nearestOnEdge; contactPoint = nearestOnEdge;
} }
} }
} }
} }
if (hasContact) { if (hasContact) {
btVector3 contactToCentre = c - contactPoint; btVector3 contactToCentre = c - contactPoint;
btScalar distanceSqr = contactToCentre.length2(); btScalar distanceSqr = contactToCentre.length2();
if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
btScalar distance = btSqrt(distanceSqr); btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre; resultNormal = contactToCentre;
resultNormal.normalize(); resultNormal.normalize();
point = contactPoint; point = contactPoint;
depth = -(r-distance); depth = -(r-distance);
return true; return true;
} }
if (delta.dot(contactToCentre) >= btScalar(0.0)) if (delta.dot(contactToCentre) >= btScalar(0.0))
return false; return false;
// Moving towards the contact point -> collision // Moving towards the contact point -> collision
point = contactPoint; point = contactPoint;
timeOfImpact = btScalar(0.0); timeOfImpact = btScalar(0.0);
return true; return true;
} }
return false; return false;
} }
bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
{ {
const btVector3* p1 = &vertices[0]; const btVector3* p1 = &vertices[0];
const btVector3* p2 = &vertices[1]; const btVector3* p2 = &vertices[1];
const btVector3* p3 = &vertices[2]; const btVector3* p3 = &vertices[2];
btVector3 edge1( *p2 - *p1 ); btVector3 edge1( *p2 - *p1 );
btVector3 edge2( *p3 - *p2 ); btVector3 edge2( *p3 - *p2 );
btVector3 edge3( *p1 - *p3 ); btVector3 edge3( *p1 - *p3 );
btVector3 p1_to_p( *p - *p1 ); btVector3 p1_to_p( *p - *p1 );
btVector3 p2_to_p( *p - *p2 ); btVector3 p2_to_p( *p - *p2 );
btVector3 p3_to_p( *p - *p3 ); btVector3 p3_to_p( *p - *p3 );
btVector3 edge1_normal( edge1.cross(normal)); btVector3 edge1_normal( edge1.cross(normal));
btVector3 edge2_normal( edge2.cross(normal)); btVector3 edge2_normal( edge2.cross(normal));
btVector3 edge3_normal( edge3.cross(normal)); btVector3 edge3_normal( edge3.cross(normal));
btScalar r1, r2, r3; btScalar r1, r2, r3;
r1 = edge1_normal.dot( p1_to_p ); r1 = edge1_normal.dot( p1_to_p );
r2 = edge2_normal.dot( p2_to_p ); r2 = edge2_normal.dot( p2_to_p );
r3 = edge3_normal.dot( p3_to_p ); r3 = edge3_normal.dot( p3_to_p );
if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
return true; return true;
return false; return false;
} }

94
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp
View File

@@ -1,47 +1,47 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btActivatingCollisionAlgorithm.h" #include "btActivatingCollisionAlgorithm.h"
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
#include "btCollisionObject.h" #include "btCollisionObject.h"
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci) btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
:btCollisionAlgorithm(ci) :btCollisionAlgorithm(ci)
//, //,
//m_colObj0(0), //m_colObj0(0),
//m_colObj1(0) //m_colObj1(0)
{ {
} }
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1) btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1)
:btCollisionAlgorithm(ci) :btCollisionAlgorithm(ci)
//, //,
//m_colObj0(0), //m_colObj0(0),
//m_colObj1(0) //m_colObj1(0)
{ {
// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1)) // if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
// { // {
// m_colObj0 = colObj0; // m_colObj0 = colObj0;
// m_colObj1 = colObj1; // m_colObj1 = colObj1;
// //
// m_colObj0->activate(); // m_colObj0->activate();
// m_colObj1->activate(); // m_colObj1->activate();
// } // }
} }
btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm() btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
{ {
// m_colObj0->activate(); // m_colObj0->activate();
// m_colObj1->activate(); // m_colObj1->activate();
} }

72
src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h
View File

@@ -1,36 +1,36 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H #ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
#define __BT_ACTIVATING_COLLISION_ALGORITHM_H #define __BT_ACTIVATING_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
///This class is not enabled yet (work-in-progress) to more aggressively activate objects. ///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
class btActivatingCollisionAlgorithm : public btCollisionAlgorithm class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
{ {
// btCollisionObject* m_colObj0; // btCollisionObject* m_colObj0;
// btCollisionObject* m_colObj1; // btCollisionObject* m_colObj1;
public: public:
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci); btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1); btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1);
virtual ~btActivatingCollisionAlgorithm(); virtual ~btActivatingCollisionAlgorithm();
}; };
#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H #endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H

170
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp
View File

@@ -1,85 +1,85 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btBoxBoxCollisionAlgorithm.h" #include "btBoxBoxCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btBoxBoxDetector.h" #include "btBoxBoxDetector.h"
#define USE_PERSISTENT_CONTACTS 1 #define USE_PERSISTENT_CONTACTS 1
btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1) btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
: btActivatingCollisionAlgorithm(ci,obj0,obj1), : btActivatingCollisionAlgorithm(ci,obj0,obj1),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf) m_manifoldPtr(mf)
{ {
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1)) if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1); m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
m_ownManifold = true; m_ownManifold = true;
} }
} }
btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm() btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
} }
void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
btCollisionObject* col0 = body0; btCollisionObject* col0 = body0;
btCollisionObject* col1 = body1; btCollisionObject* col1 = body1;
btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape(); btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape(); btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
#ifndef USE_PERSISTENT_CONTACTS #ifndef USE_PERSISTENT_CONTACTS
m_manifoldPtr->clearManifold(); m_manifoldPtr->clearManifold();
#endif //USE_PERSISTENT_CONTACTS #endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface::ClosestPointInput input; btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = 1e30f; input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform(); input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform(); input.m_transformB = body1->getWorldTransform();
btBoxBoxDetector detector(box0,box1); btBoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#ifdef USE_PERSISTENT_CONTACTS #ifdef USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added // refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold) if (m_ownManifold)
{ {
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
} }
#endif //USE_PERSISTENT_CONTACTS #endif //USE_PERSISTENT_CONTACTS
} }
btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/) btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
{ {
//not yet //not yet
return 1.f; return 1.f;
} }

132
src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h
View File

@@ -1,66 +1,66 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BOX_BOX__COLLISION_ALGORITHM_H #ifndef BOX_BOX__COLLISION_ALGORITHM_H
#define BOX_BOX__COLLISION_ALGORITHM_H #define BOX_BOX__COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h" #include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold; class btPersistentManifold;
///box-box collision detection ///box-box collision detection
class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
public: public:
btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {} : btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
virtual ~btBoxBoxCollisionAlgorithm(); virtual ~btBoxBoxCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr && m_ownManifold) if (m_manifoldPtr && m_ownManifold)
{ {
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
} }
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
int bbsize = sizeof(btBoxBoxCollisionAlgorithm); int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize); void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1); return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
} }
}; };
}; };
#endif //BOX_BOX__COLLISION_ALGORITHM_H #endif //BOX_BOX__COLLISION_ALGORITHM_H

1378
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp
View File

File diff suppressed because it is too large Load Diff

88
src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h
View File

@@ -1,44 +1,44 @@
/* /*
* Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith * 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. * 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 * All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BOX_BOX_DETECTOR_H #ifndef BOX_BOX_DETECTOR_H
#define BOX_BOX_DETECTOR_H #define BOX_BOX_DETECTOR_H
class btBoxShape; class btBoxShape;
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
/// btBoxBoxDetector wraps the ODE box-box collision detector /// btBoxBoxDetector wraps the ODE box-box collision detector
/// re-distributed under the Zlib license with permission from Russell L. Smith /// re-distributed under the Zlib license with permission from Russell L. Smith
struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
{ {
btBoxShape* m_box1; btBoxShape* m_box1;
btBoxShape* m_box2; btBoxShape* m_box2;
public: public:
btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2); btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
virtual ~btBoxBoxDetector() {}; virtual ~btBoxBoxDetector() {};
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false); virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
}; };
#endif //BT_BOX_BOX_DETECTOR_H #endif //BT_BOX_BOX_DETECTOR_H

310
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
View File

@@ -1,155 +1,155 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btConvexPlaneCollisionAlgorithm.h" #include "btConvexPlaneCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
//#include <stdio.h> //#include <stdio.h>
btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold) btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
: btCollisionAlgorithm(ci), : btCollisionAlgorithm(ci),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf), m_manifoldPtr(mf),
m_isSwapped(isSwapped), m_isSwapped(isSwapped),
m_numPerturbationIterations(numPerturbationIterations), m_numPerturbationIterations(numPerturbationIterations),
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{ {
btCollisionObject* convexObj = m_isSwapped? col1 : col0; btCollisionObject* convexObj = m_isSwapped? col1 : col0;
btCollisionObject* planeObj = m_isSwapped? col0 : col1; btCollisionObject* planeObj = m_isSwapped? col0 : col1;
if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj)) if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj); m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj);
m_ownManifold = true; m_ownManifold = true;
} }
} }
btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm() btConvexPlaneCollisionAlgorithm::~btConvexPlaneCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
} }
void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* convexObj = m_isSwapped? body1 : body0;
btCollisionObject* planeObj = m_isSwapped? body0: body1; btCollisionObject* planeObj = m_isSwapped? body0: body1;
btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
bool hasCollision = false; bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal(); const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant(); const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = convexObj->getWorldTransform(); btTransform convexWorldTransform = convexObj->getWorldTransform();
btTransform convexInPlaneTrans; btTransform convexInPlaneTrans;
convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform; convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
//now perturbe the convex-world transform //now perturbe the convex-world transform
convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot); convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
btTransform planeInConvex; btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform(); planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform();
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx); btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision) if (hasCollision)
{ {
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal;
btVector3 pOnB = vtxInPlaneWorld; btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
} }
} }
void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)dispatchInfo; (void)dispatchInfo;
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* convexObj = m_isSwapped? body1 : body0;
btCollisionObject* planeObj = m_isSwapped? body0: body1; btCollisionObject* planeObj = m_isSwapped? body0: body1;
btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
bool hasCollision = false; bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal(); const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant(); const btScalar& planeConstant = planeShape->getPlaneConstant();
//first perform a collision query with the non-perturbated collision objects //first perform a collision query with the non-perturbated collision objects
{ {
btQuaternion rotq(0,0,0,1); btQuaternion rotq(0,0,0,1);
collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
} }
if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
{ {
btVector3 v0,v1; btVector3 v0,v1;
btPlaneSpace1(planeNormal,v0,v1); btPlaneSpace1(planeNormal,v0,v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
const btScalar angleLimit = 0.125f * SIMD_PI; const btScalar angleLimit = 0.125f * SIMD_PI;
btScalar perturbeAngle; btScalar perturbeAngle;
btScalar radius = convexShape->getAngularMotionDisc(); btScalar radius = convexShape->getAngularMotionDisc();
perturbeAngle = gContactBreakingThreshold / radius; perturbeAngle = gContactBreakingThreshold / radius;
if ( perturbeAngle > angleLimit ) if ( perturbeAngle > angleLimit )
perturbeAngle = angleLimit; perturbeAngle = angleLimit;
btQuaternion perturbeRot(v0,perturbeAngle); btQuaternion perturbeRot(v0,perturbeAngle);
for (int i=0;i<m_numPerturbationIterations;i++) for (int i=0;i<m_numPerturbationIterations;i++)
{ {
btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
btQuaternion rotq(planeNormal,iterationAngle); btQuaternion rotq(planeNormal,iterationAngle);
collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut); collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
} }
} }
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr->getNumContacts()) if (m_manifoldPtr->getNumContacts())
{ {
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
} }
} }
} }
btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar btConvexPlaneCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)resultOut; (void)resultOut;
(void)dispatchInfo; (void)dispatchInfo;
(void)col0; (void)col0;
(void)col1; (void)col1;
//not yet //not yet
return btScalar(1.); return btScalar(1.);
} }

168
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
View File

@@ -1,84 +1,84 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H #ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H
#define CONVEX_PLANE_COLLISION_ALGORITHM_H #define CONVEX_PLANE_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold; class btPersistentManifold;
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response. /// Other features are frame-coherency (persistent data) and collision response.
class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
bool m_isSwapped; bool m_isSwapped;
int m_numPerturbationIterations; int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold; int m_minimumPointsPerturbationThreshold;
public: public:
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold); btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm(); virtual ~btConvexPlaneCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr && m_ownManifold) if (m_manifoldPtr && m_ownManifold)
{ {
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
} }
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
int m_numPerturbationIterations; int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold; int m_minimumPointsPerturbationThreshold;
CreateFunc() CreateFunc()
: m_numPerturbationIterations(3), : m_numPerturbationIterations(3),
m_minimumPointsPerturbationThreshold(3) m_minimumPointsPerturbationThreshold(3)
{ {
} }
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
if (!m_swapped) if (!m_swapped)
{ {
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} else } else
{ {
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} }
} }
}; };
}; };
#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H #endif //CONVEX_PLANE_COLLISION_ALGORITHM_H

340
src/BulletCollision/CollisionDispatch/btGhostObject.cpp
View File

@@ -1,170 +1,170 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btGhostObject.h" #include "btGhostObject.h"
#include "btCollisionWorld.h" #include "btCollisionWorld.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/btAabbUtil2.h" #include "LinearMath/btAabbUtil2.h"
btGhostObject::btGhostObject() btGhostObject::btGhostObject()
{ {
m_internalType = CO_GHOST_OBJECT; m_internalType = CO_GHOST_OBJECT;
} }
btGhostObject::~btGhostObject() btGhostObject::~btGhostObject()
{ {
///btGhostObject should have been removed from the world, so no overlapping objects ///btGhostObject should have been removed from the world, so no overlapping objects
btAssert(!m_overlappingObjects.size()); btAssert(!m_overlappingObjects.size());
} }
void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) void btGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
{ {
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject); btAssert(otherObject);
///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure ///if this linearSearch becomes too slow (too many overlapping objects) we should add a more appropriate data structure
int index = m_overlappingObjects.findLinearSearch(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index==m_overlappingObjects.size()) if (index==m_overlappingObjects.size())
{ {
//not found //not found
m_overlappingObjects.push_back(otherObject); m_overlappingObjects.push_back(otherObject);
} }
} }
void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy) void btGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy)
{ {
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject); btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index<m_overlappingObjects.size()) if (index<m_overlappingObjects.size())
{ {
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
m_overlappingObjects.pop_back(); m_overlappingObjects.pop_back();
} }
} }
btPairCachingGhostObject::btPairCachingGhostObject() btPairCachingGhostObject::btPairCachingGhostObject()
{ {
m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); m_hashPairCache = new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
} }
btPairCachingGhostObject::~btPairCachingGhostObject() btPairCachingGhostObject::~btPairCachingGhostObject()
{ {
btAlignedFree( m_hashPairCache ); btAlignedFree( m_hashPairCache );
} }
void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy) void btPairCachingGhostObject::addOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btBroadphaseProxy* thisProxy)
{ {
btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle(); btBroadphaseProxy*actualThisProxy = thisProxy ? thisProxy : getBroadphaseHandle();
btAssert(actualThisProxy); btAssert(actualThisProxy);
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btAssert(otherObject); btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index==m_overlappingObjects.size()) if (index==m_overlappingObjects.size())
{ {
m_overlappingObjects.push_back(otherObject); m_overlappingObjects.push_back(otherObject);
m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy); m_hashPairCache->addOverlappingPair(actualThisProxy,otherProxy);
} }
} }
void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1) void btPairCachingGhostObject::removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy1)
{ {
btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject; btCollisionObject* otherObject = (btCollisionObject*)otherProxy->m_clientObject;
btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle(); btBroadphaseProxy* actualThisProxy = thisProxy1 ? thisProxy1 : getBroadphaseHandle();
btAssert(actualThisProxy); btAssert(actualThisProxy);
btAssert(otherObject); btAssert(otherObject);
int index = m_overlappingObjects.findLinearSearch(otherObject); int index = m_overlappingObjects.findLinearSearch(otherObject);
if (index<m_overlappingObjects.size()) if (index<m_overlappingObjects.size())
{ {
m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1]; m_overlappingObjects[index] = m_overlappingObjects[m_overlappingObjects.size()-1];
m_overlappingObjects.pop_back(); m_overlappingObjects.pop_back();
m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher); m_hashPairCache->removeOverlappingPair(actualThisProxy,otherProxy,dispatcher);
} }
} }
void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const void btGhostObject::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const
{ {
btTransform convexFromTrans,convexToTrans; btTransform convexFromTrans,convexToTrans;
convexFromTrans = convexFromWorld; convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld; convexToTrans = convexToWorld;
btVector3 castShapeAabbMin, castShapeAabbMax; btVector3 castShapeAabbMin, castShapeAabbMax;
/* Compute AABB that encompasses angular movement */ /* Compute AABB that encompasses angular movement */
{ {
btVector3 linVel, angVel; btVector3 linVel, angVel;
btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel);
btTransform R; btTransform R;
R.setIdentity (); R.setIdentity ();
R.setRotation (convexFromTrans.getRotation()); R.setRotation (convexFromTrans.getRotation());
castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); castShape->calculateTemporalAabb (R, linVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax);
} }
/// go over all objects, and if the ray intersects their aabb + cast shape aabb, /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD) // do a ray-shape query using convexCaster (CCD)
int i; int i;
for (i=0;i<m_overlappingObjects.size();i++) for (i=0;i<m_overlappingObjects.size();i++)
{ {
btCollisionObject* collisionObject= m_overlappingObjects[i]; btCollisionObject* collisionObject= m_overlappingObjects[i];
//only perform raycast if filterMask matches //only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax; btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal; btVector3 hitNormal;
if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{ {
btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans, btCollisionWorld::objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
resultCallback, resultCallback,
allowedCcdPenetration); allowedCcdPenetration);
} }
} }
} }
} }
void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const void btGhostObject::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const
{ {
btTransform rayFromTrans; btTransform rayFromTrans;
rayFromTrans.setIdentity(); rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld); rayFromTrans.setOrigin(rayFromWorld);
btTransform rayToTrans; btTransform rayToTrans;
rayToTrans.setIdentity(); rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld); rayToTrans.setOrigin(rayToWorld);
int i; int i;
for (i=0;i<m_overlappingObjects.size();i++) for (i=0;i<m_overlappingObjects.size();i++)
{ {
btCollisionObject* collisionObject= m_overlappingObjects[i]; btCollisionObject* collisionObject= m_overlappingObjects[i];
//only perform raycast if filterMask matches //only perform raycast if filterMask matches
if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle()))
{ {
btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,
collisionObject, collisionObject,
collisionObject->getCollisionShape(), collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(), collisionObject->getWorldTransform(),
resultCallback); resultCallback);
} }
} }
} }

346
src/BulletCollision/CollisionDispatch/btGhostObject.h
View File

@@ -1,174 +1,174 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_GHOST_OBJECT_H #ifndef BT_GHOST_OBJECT_H
#define BT_GHOST_OBJECT_H #define BT_GHOST_OBJECT_H
#include "btCollisionObject.h" #include "btCollisionObject.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "btCollisionWorld.h" #include "btCollisionWorld.h"
class btConvexShape; class btConvexShape;
class btDispatcher; class btDispatcher;
///The btGhostObject can keep track of all objects that are overlapping ///The btGhostObject can keep track of all objects that are overlapping
///By default, this overlap is based on the AABB ///By default, this overlap is based on the AABB
///This is useful for creating a character controller, collision sensors/triggers, explosions etc. ///This is useful for creating a character controller, collision sensors/triggers, explosions etc.
///We plan on adding rayTest and other queries for the btGhostObject ///We plan on adding rayTest and other queries for the btGhostObject
ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject ATTRIBUTE_ALIGNED16(class) btGhostObject : public btCollisionObject
{ {
protected: protected:
btAlignedObjectArray<btCollisionObject*> m_overlappingObjects; btAlignedObjectArray<btCollisionObject*> m_overlappingObjects;
public: public:
btGhostObject(); btGhostObject();
virtual ~btGhostObject(); virtual ~btGhostObject();
void convexSweepTest(const class btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, btCollisionWorld::ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = 0.f) const; 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; void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, btCollisionWorld::RayResultCallback& resultCallback) const;
///this method is mainly for expert/internal use only. ///this method is mainly for expert/internal use only.
virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
///this method is mainly for expert/internal use only. ///this method is mainly for expert/internal use only.
virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
int getNumOverlappingObjects() const int getNumOverlappingObjects() const
{ {
return m_overlappingObjects.size(); return m_overlappingObjects.size();
} }
btCollisionObject* getOverlappingObject(int index) btCollisionObject* getOverlappingObject(int index)
{ {
return m_overlappingObjects[index]; return m_overlappingObjects[index];
} }
const btCollisionObject* getOverlappingObject(int index) const const btCollisionObject* getOverlappingObject(int index) const
{ {
return m_overlappingObjects[index]; return m_overlappingObjects[index];
} }
btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs() btAlignedObjectArray<btCollisionObject*>& getOverlappingPairs()
{ {
return m_overlappingObjects; return m_overlappingObjects;
} }
const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const const btAlignedObjectArray<btCollisionObject*> getOverlappingPairs() const
{ {
return m_overlappingObjects; return m_overlappingObjects;
} }
// //
// internal cast // internal cast
// //
static const btGhostObject* upcast(const btCollisionObject* colObj) static const btGhostObject* upcast(const btCollisionObject* colObj)
{ {
if (colObj->getInternalType()==CO_GHOST_OBJECT) if (colObj->getInternalType()==CO_GHOST_OBJECT)
return (const btGhostObject*)colObj; return (const btGhostObject*)colObj;
return 0; return 0;
} }
static btGhostObject* upcast(btCollisionObject* colObj) static btGhostObject* upcast(btCollisionObject* colObj)
{ {
if (colObj->getInternalType()==CO_GHOST_OBJECT) if (colObj->getInternalType()==CO_GHOST_OBJECT)
return (btGhostObject*)colObj; return (btGhostObject*)colObj;
return 0; return 0;
} }
}; };
class btPairCachingGhostObject : public btGhostObject class btPairCachingGhostObject : public btGhostObject
{ {
btHashedOverlappingPairCache* m_hashPairCache; btHashedOverlappingPairCache* m_hashPairCache;
public: public:
btPairCachingGhostObject(); btPairCachingGhostObject();
virtual ~btPairCachingGhostObject(); virtual ~btPairCachingGhostObject();
///this method is mainly for expert/internal use only. ///this method is mainly for expert/internal use only.
virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0); virtual void addOverlappingObjectInternal(btBroadphaseProxy* otherProxy, btBroadphaseProxy* thisProxy=0);
virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0); virtual void removeOverlappingObjectInternal(btBroadphaseProxy* otherProxy,btDispatcher* dispatcher,btBroadphaseProxy* thisProxy=0);
btHashedOverlappingPairCache* getOverlappingPairCache() btHashedOverlappingPairCache* getOverlappingPairCache()
{ {
return m_hashPairCache; return m_hashPairCache;
} }
}; };
///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject. ///The btGhostPairCallback interfaces and forwards adding and removal of overlapping pairs from the btBroadphaseInterface to btGhostObject.
class btGhostPairCallback : public btOverlappingPairCallback class btGhostPairCallback : public btOverlappingPairCallback
{ {
public: public:
btGhostPairCallback() btGhostPairCallback()
{ {
} }
virtual ~btGhostPairCallback() virtual ~btGhostPairCallback()
{ {
} }
virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{ {
btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0); btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1); btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
if (ghost0) if (ghost0)
ghost0->addOverlappingObjectInternal(proxy1, proxy0); ghost0->addOverlappingObjectInternal(proxy1, proxy0);
if (ghost1) if (ghost1)
ghost1->addOverlappingObjectInternal(proxy0, proxy1); ghost1->addOverlappingObjectInternal(proxy0, proxy1);
return 0; return 0;
} }
virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher) virtual void* removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
{ {
btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject; btCollisionObject* colObj0 = (btCollisionObject*) proxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*) proxy1->m_clientObject;
btGhostObject* ghost0 = btGhostObject::upcast(colObj0); btGhostObject* ghost0 = btGhostObject::upcast(colObj0);
btGhostObject* ghost1 = btGhostObject::upcast(colObj1); btGhostObject* ghost1 = btGhostObject::upcast(colObj1);
if (ghost0) if (ghost0)
ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0); ghost0->removeOverlappingObjectInternal(proxy1,dispatcher,proxy0);
if (ghost1) if (ghost1)
ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1); ghost1->removeOverlappingObjectInternal(proxy0,dispatcher,proxy1);
return 0; return 0;
} }
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher) virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy0,btDispatcher* dispatcher)
{ {
btAssert(0); btAssert(0);
//need to keep track of all ghost objects and call them here //need to keep track of all ghost objects and call them here
//m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher); //m_hashPairCache->removeOverlappingPairsContainingProxy(proxy0,dispatcher);
} }
}; };
#endif #endif

780
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -1,390 +1,390 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "btSimulationIslandManager.h" #include "btSimulationIslandManager.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
//#include <stdio.h> //#include <stdio.h>
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
btSimulationIslandManager::btSimulationIslandManager(): btSimulationIslandManager::btSimulationIslandManager():
m_splitIslands(true) m_splitIslands(true)
{ {
} }
btSimulationIslandManager::~btSimulationIslandManager() btSimulationIslandManager::~btSimulationIslandManager()
{ {
} }
void btSimulationIslandManager::initUnionFind(int n) void btSimulationIslandManager::initUnionFind(int n)
{ {
m_unionFind.reset(n); m_unionFind.reset(n);
} }
void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld) void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
{ {
{ {
for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++) for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
{ {
btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr(); btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
const btBroadphasePair& collisionPair = pairPtr[i]; const btBroadphasePair& collisionPair = pairPtr[i];
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject; btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands()))) ((colObj1) && ((colObj1)->mergesSimulationIslands())))
{ {
m_unionFind.unite((colObj0)->getIslandTag(), m_unionFind.unite((colObj0)->getIslandTag(),
(colObj1)->getIslandTag()); (colObj1)->getIslandTag());
} }
} }
} }
} }
void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
{ {
initUnionFind( int (colWorld->getCollisionObjectArray().size())); initUnionFind( int (colWorld->getCollisionObjectArray().size()));
// put the index into m_controllers into m_tag // put the index into m_controllers into m_tag
{ {
int index = 0; int index = 0;
int i; int i;
for (i=0;i<colWorld->getCollisionObjectArray().size(); i++) for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
{ {
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
collisionObject->setIslandTag(index); collisionObject->setIslandTag(index);
collisionObject->setCompanionId(-1); collisionObject->setCompanionId(-1);
collisionObject->setHitFraction(btScalar(1.)); collisionObject->setHitFraction(btScalar(1.));
index++; index++;
} }
} }
// do the union find // do the union find
findUnions(dispatcher,colWorld); findUnions(dispatcher,colWorld);
} }
void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
{ {
// put the islandId ('find' value) into m_tag // put the islandId ('find' value) into m_tag
{ {
int index = 0; int index = 0;
int i; int i;
for (i=0;i<colWorld->getCollisionObjectArray().size();i++) for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
{ {
btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
if (!collisionObject->isStaticOrKinematicObject()) if (!collisionObject->isStaticOrKinematicObject())
{ {
collisionObject->setIslandTag( m_unionFind.find(index) ); collisionObject->setIslandTag( m_unionFind.find(index) );
collisionObject->setCompanionId(-1); collisionObject->setCompanionId(-1);
} else } else
{ {
collisionObject->setIslandTag(-1); collisionObject->setIslandTag(-1);
collisionObject->setCompanionId(-2); collisionObject->setCompanionId(-2);
} }
index++; index++;
} }
} }
} }
inline int getIslandId(const btPersistentManifold* lhs) inline int getIslandId(const btPersistentManifold* lhs)
{ {
int islandId; int islandId;
const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag(); islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
return islandId; return islandId;
} }
/// function object that routes calls to operator< /// function object that routes calls to operator<
class btPersistentManifoldSortPredicate class btPersistentManifoldSortPredicate
{ {
public: public:
SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs )
{ {
return getIslandId(lhs) < getIslandId(rhs); return getIslandId(lhs) < getIslandId(rhs);
} }
}; };
void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
{ {
BT_PROFILE("islandUnionFindAndQuickSort"); BT_PROFILE("islandUnionFindAndQuickSort");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
m_islandmanifold.resize(0); m_islandmanifold.resize(0);
//we are going to sort the unionfind array, and store the element id in the size //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 //afterwards, we clean unionfind, to make sure no-one uses it anymore
getUnionFind().sortIslands(); getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements(); int numElem = getUnionFind().getNumElements();
int endIslandIndex=1; int endIslandIndex=1;
int startIslandIndex; int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping //update the sleeping state for bodies, if all are sleeping
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{ {
int islandId = getUnionFind().getElement(startIslandIndex).m_id; int islandId = getUnionFind().getElement(startIslandIndex).m_id;
for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{ {
} }
//int numSleeping = 0; //int numSleeping = 0;
bool allSleeping = true; bool allSleeping = true;
int idx; int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++) for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{ {
int i = getUnionFind().getElement(idx).m_sz; int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{ {
// printf("error in island management\n"); // printf("error in island management\n");
} }
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId) if (colObj0->getIslandTag() == islandId)
{ {
if (colObj0->getActivationState()== ACTIVE_TAG) if (colObj0->getActivationState()== ACTIVE_TAG)
{ {
allSleeping = false; allSleeping = false;
} }
if (colObj0->getActivationState()== DISABLE_DEACTIVATION) if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
{ {
allSleeping = false; allSleeping = false;
} }
} }
} }
if (allSleeping) if (allSleeping)
{ {
int idx; int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++) for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{ {
int i = getUnionFind().getElement(idx).m_sz; int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{ {
// printf("error in island management\n"); // printf("error in island management\n");
} }
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId) if (colObj0->getIslandTag() == islandId)
{ {
colObj0->setActivationState( ISLAND_SLEEPING ); colObj0->setActivationState( ISLAND_SLEEPING );
} }
} }
} else } else
{ {
int idx; int idx;
for (idx=startIslandIndex;idx<endIslandIndex;idx++) for (idx=startIslandIndex;idx<endIslandIndex;idx++)
{ {
int i = getUnionFind().getElement(idx).m_sz; int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{ {
// printf("error in island management\n"); // printf("error in island management\n");
} }
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId) if (colObj0->getIslandTag() == islandId)
{ {
if ( colObj0->getActivationState() == ISLAND_SLEEPING) if ( colObj0->getActivationState() == ISLAND_SLEEPING)
{ {
colObj0->setActivationState( WANTS_DEACTIVATION); colObj0->setActivationState( WANTS_DEACTIVATION);
colObj0->setDeactivationTime(0.f); colObj0->setDeactivationTime(0.f);
} }
} }
} }
} }
} }
int i; int i;
int maxNumManifolds = dispatcher->getNumManifolds(); int maxNumManifolds = dispatcher->getNumManifolds();
//#define SPLIT_ISLANDS 1 //#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS //#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS //#endif //SPLIT_ISLANDS
for (i=0;i<maxNumManifolds ;i++) for (i=0;i<maxNumManifolds ;i++)
{ {
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0()); btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1()); btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->getBody1());
///@todo: check sleeping conditions! ///@todo: check sleeping conditions!
if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
{ {
//kinematic objects don't merge islands, but wake up all connected objects //kinematic objects don't merge islands, but wake up all connected objects
if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
{ {
colObj1->activate(); colObj1->activate();
} }
if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
{ {
colObj0->activate(); colObj0->activate();
} }
if(m_splitIslands) if(m_splitIslands)
{ {
//filtering for response //filtering for response
if (dispatcher->needsResponse(colObj0,colObj1)) if (dispatcher->needsResponse(colObj0,colObj1))
m_islandmanifold.push_back(manifold); m_islandmanifold.push_back(manifold);
} }
} }
} }
} }
///@todo: this is random access, it can be walked 'cache friendly'! ///@todo: this is random access, it can be walked 'cache friendly'!
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback) void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
{ {
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
buildIslands(dispatcher,collisionWorld); buildIslands(dispatcher,collisionWorld);
int endIslandIndex=1; int endIslandIndex=1;
int startIslandIndex; int startIslandIndex;
int numElem = getUnionFind().getNumElements(); int numElem = getUnionFind().getNumElements();
BT_PROFILE("processIslands"); BT_PROFILE("processIslands");
if(!m_splitIslands) if(!m_splitIslands)
{ {
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds(); int maxNumManifolds = dispatcher->getNumManifolds();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
} }
else else
{ {
// Sort manifolds, based on islands // Sort manifolds, based on islands
// Sort the vector using predicate and std::sort // Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = int (m_islandmanifold.size()); int numManifolds = int (m_islandmanifold.size());
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now) //now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0; int startManifoldIndex = 0;
int endManifoldIndex = 1; int endManifoldIndex = 1;
//int islandId; //int islandId;
// printf("Start Islands\n"); // printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex) for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{ {
int islandId = getUnionFind().getElement(startIslandIndex).m_id; int islandId = getUnionFind().getElement(startIslandIndex).m_id;
bool islandSleeping = false; bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++) for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{ {
int i = getUnionFind().getElement(endIslandIndex).m_sz; int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i]; btCollisionObject* colObj0 = collisionObjects[i];
m_islandBodies.push_back(colObj0); m_islandBodies.push_back(colObj0);
if (!colObj0->isActive()) if (!colObj0->isActive())
islandSleeping = true; islandSleeping = true;
} }
//find the accompanying contact manifold for this islandId //find the accompanying contact manifold for this islandId
int numIslandManifolds = 0; int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0; btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds) if (startManifoldIndex<numManifolds)
{ {
int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]); int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId) if (curIslandId == islandId)
{ {
startManifold = &m_islandmanifold[startManifoldIndex]; startManifold = &m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++) for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{ {
} }
/// Process the actual simulation, only if not sleeping/deactivated /// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex; numIslandManifolds = endManifoldIndex-startManifoldIndex;
} }
} }
if (!islandSleeping) if (!islandSleeping)
{ {
callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
} }
if (numIslandManifolds) if (numIslandManifolds)
{ {
startManifoldIndex = endManifoldIndex; startManifoldIndex = endManifoldIndex;
} }
m_islandBodies.resize(0); m_islandBodies.resize(0);
} }
} // else if(!splitIslands) } // else if(!splitIslands)
} }

520
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp
View File

@@ -1,260 +1,260 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btSphereBoxCollisionAlgorithm.h" #include "btSphereBoxCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
//#include <stdio.h> //#include <stdio.h>
btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped) btSphereBoxCollisionAlgorithm::btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
: btActivatingCollisionAlgorithm(ci,col0,col1), : btActivatingCollisionAlgorithm(ci,col0,col1),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf), m_manifoldPtr(mf),
m_isSwapped(isSwapped) m_isSwapped(isSwapped)
{ {
btCollisionObject* sphereObj = m_isSwapped? col1 : col0; btCollisionObject* sphereObj = m_isSwapped? col1 : col0;
btCollisionObject* boxObj = m_isSwapped? col0 : col1; btCollisionObject* boxObj = m_isSwapped? col0 : col1;
if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj)) if (!m_manifoldPtr && m_dispatcher->needsCollision(sphereObj,boxObj))
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj); m_manifoldPtr = m_dispatcher->getNewManifold(sphereObj,boxObj);
m_ownManifold = true; m_ownManifold = true;
} }
} }
btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm() btSphereBoxCollisionAlgorithm::~btSphereBoxCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
} }
void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btSphereBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)dispatchInfo; (void)dispatchInfo;
(void)resultOut; (void)resultOut;
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
btCollisionObject* sphereObj = m_isSwapped? body1 : body0; btCollisionObject* sphereObj = m_isSwapped? body1 : body0;
btCollisionObject* boxObj = m_isSwapped? body0 : body1; btCollisionObject* boxObj = m_isSwapped? body0 : body1;
btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape(); btSphereShape* sphere0 = (btSphereShape*)sphereObj->getCollisionShape();
btVector3 normalOnSurfaceB; btVector3 normalOnSurfaceB;
btVector3 pOnBox,pOnSphere; btVector3 pOnBox,pOnSphere;
btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin(); btVector3 sphereCenter = sphereObj->getWorldTransform().getOrigin();
btScalar radius = sphere0->getRadius(); btScalar radius = sphere0->getRadius();
btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); btScalar dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius);
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
if (dist < SIMD_EPSILON) if (dist < SIMD_EPSILON)
{ {
btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize(); btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize();
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist); resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist);
} }
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr->getNumContacts()) if (m_manifoldPtr->getNumContacts())
{ {
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
} }
} }
} }
btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)resultOut; (void)resultOut;
(void)dispatchInfo; (void)dispatchInfo;
(void)col0; (void)col0;
(void)col1; (void)col1;
//not yet //not yet
return btScalar(1.); return btScalar(1.);
} }
btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius )
{ {
btScalar margins; btScalar margins;
btVector3 bounds[2]; btVector3 bounds[2];
btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape(); btBoxShape* boxShape= (btBoxShape*)boxObj->getCollisionShape();
bounds[0] = -boxShape->getHalfExtentsWithoutMargin(); bounds[0] = -boxShape->getHalfExtentsWithoutMargin();
bounds[1] = boxShape->getHalfExtentsWithoutMargin(); bounds[1] = boxShape->getHalfExtentsWithoutMargin();
margins = boxShape->getMargin();//also add sphereShape margin? margins = boxShape->getMargin();//also add sphereShape margin?
const btTransform& m44T = boxObj->getWorldTransform(); const btTransform& m44T = boxObj->getWorldTransform();
btVector3 boundsVec[2]; btVector3 boundsVec[2];
btScalar fPenetration; btScalar fPenetration;
boundsVec[0] = bounds[0]; boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1]; boundsVec[1] = bounds[1];
btVector3 marginsVec( margins, margins, margins ); btVector3 marginsVec( margins, margins, margins );
// add margins // add margins
bounds[0] += marginsVec; bounds[0] += marginsVec;
bounds[1] -= marginsVec; bounds[1] -= marginsVec;
///////////////////////////////////////////////// /////////////////////////////////////////////////
btVector3 tmp, prel, n[6], normal, v3P; btVector3 tmp, prel, n[6], normal, v3P;
btScalar fSep = btScalar(10000000.0), fSepThis; btScalar fSep = btScalar(10000000.0), fSepThis;
n[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.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[1].setValue( btScalar(0.0), btScalar(-1.0), btScalar(0.0) );
n[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.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[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.0) );
n[4].setValue( btScalar(0.0), btScalar(1.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) ); n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) );
// convert point in local space // convert point in local space
prel = m44T.invXform( sphereCenter); prel = m44T.invXform( sphereCenter);
bool bFound = false; bool bFound = false;
v3P = prel; v3P = prel;
for (int i=0;i<6;i++) for (int i=0;i<6;i++)
{ {
int j = i<3? 0:1; int j = i<3? 0:1;
if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) ) if ( (fSepThis = ((v3P-bounds[j]) .dot(n[i]))) > btScalar(0.0) )
{ {
v3P = v3P - n[i]*fSepThis; v3P = v3P - n[i]*fSepThis;
bFound = true; bFound = true;
} }
} }
// //
if ( bFound ) if ( bFound )
{ {
bounds[0] = boundsVec[0]; bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1]; bounds[1] = boundsVec[1];
normal = (prel - v3P).normalize(); normal = (prel - v3P).normalize();
pointOnBox = v3P + normal*margins; pointOnBox = v3P + normal*margins;
v3PointOnSphere = prel - normal*fRadius; v3PointOnSphere = prel - normal*fRadius;
if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) ) if ( ((v3PointOnSphere - pointOnBox) .dot (normal)) > btScalar(0.0) )
{ {
return btScalar(1.0); return btScalar(1.0);
} }
// transform back in world space // transform back in world space
tmp = m44T( pointOnBox); tmp = m44T( pointOnBox);
pointOnBox = tmp; pointOnBox = tmp;
tmp = m44T( v3PointOnSphere); tmp = m44T( v3PointOnSphere);
v3PointOnSphere = tmp; v3PointOnSphere = tmp;
btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2(); btScalar fSeps2 = (pointOnBox-v3PointOnSphere).length2();
//if this fails, fallback into deeper penetration case, below //if this fails, fallback into deeper penetration case, below
if (fSeps2 > SIMD_EPSILON) if (fSeps2 > SIMD_EPSILON)
{ {
fSep = - btSqrt(fSeps2); fSep = - btSqrt(fSeps2);
normal = (pointOnBox-v3PointOnSphere); normal = (pointOnBox-v3PointOnSphere);
normal *= btScalar(1.)/fSep; normal *= btScalar(1.)/fSep;
} }
return fSep; return fSep;
} }
////////////////////////////////////////////////// //////////////////////////////////////////////////
// Deep penetration case // Deep penetration case
fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] ); fPenetration = getSpherePenetration( boxObj,pointOnBox, v3PointOnSphere, sphereCenter, fRadius,bounds[0],bounds[1] );
bounds[0] = boundsVec[0]; bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1]; bounds[1] = boundsVec[1];
if ( fPenetration <= btScalar(0.0) ) if ( fPenetration <= btScalar(0.0) )
return (fPenetration-margins); return (fPenetration-margins);
else else
return btScalar(1.0); return btScalar(1.0);
} }
btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax) btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btCollisionObject* boxObj,btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax)
{ {
btVector3 bounds[2]; btVector3 bounds[2];
bounds[0] = aabbMin; bounds[0] = aabbMin;
bounds[1] = aabbMax; bounds[1] = aabbMax;
btVector3 p0, tmp, prel, n[6], normal; btVector3 p0, tmp, prel, n[6], normal;
btScalar fSep = btScalar(-10000000.0), fSepThis; btScalar fSep = btScalar(-10000000.0), fSepThis;
// set p0 and normal to a default value to shup up GCC // set p0 and normal to a default value to shup up GCC
p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); p0.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
normal.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[0].setValue( btScalar(-1.0), btScalar(0.0), btScalar(0.0) );
n[1].setValue( btScalar(0.0), btScalar(-1.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[2].setValue( btScalar(0.0), btScalar(0.0), btScalar(-1.0) );
n[3].setValue( btScalar(1.0), btScalar(0.0), btScalar(0.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[4].setValue( btScalar(0.0), btScalar(1.0), btScalar(0.0) );
n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) ); n[5].setValue( btScalar(0.0), btScalar(0.0), btScalar(1.0) );
const btTransform& m44T = boxObj->getWorldTransform(); const btTransform& m44T = boxObj->getWorldTransform();
// convert point in local space // convert point in local space
prel = m44T.invXform( sphereCenter); prel = m44T.invXform( sphereCenter);
/////////// ///////////
for (int i=0;i<6;i++) for (int i=0;i<6;i++)
{ {
int j = i<3 ? 0:1; int j = i<3 ? 0:1;
if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0); if ( (fSepThis = ((prel-bounds[j]) .dot( n[i]))-fRadius) > btScalar(0.0) ) return btScalar(1.0);
if ( fSepThis > fSep ) if ( fSepThis > fSep )
{ {
p0 = bounds[j]; normal = (btVector3&)n[i]; p0 = bounds[j]; normal = (btVector3&)n[i];
fSep = fSepThis; fSep = fSepThis;
} }
} }
pointOnBox = prel - normal*(normal.dot((prel-p0))); pointOnBox = prel - normal*(normal.dot((prel-p0)));
v3PointOnSphere = pointOnBox + normal*fSep; v3PointOnSphere = pointOnBox + normal*fSep;
// transform back in world space // transform back in world space
tmp = m44T( pointOnBox); tmp = m44T( pointOnBox);
pointOnBox = tmp; pointOnBox = tmp;
tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp;
normal = (pointOnBox-v3PointOnSphere).normalize(); normal = (pointOnBox-v3PointOnSphere).normalize();
return fSep; return fSep;
} }

150
src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h
View File

@@ -1,75 +1,75 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H #ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
#define SPHERE_BOX_COLLISION_ALGORITHM_H #define SPHERE_BOX_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h" #include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold; class btPersistentManifold;
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response. /// Other features are frame-coherency (persistent data) and collision response.
class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
bool m_isSwapped; bool m_isSwapped;
public: public:
btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
virtual ~btSphereBoxCollisionAlgorithm(); virtual ~btSphereBoxCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr && m_ownManifold) if (m_manifoldPtr && m_ownManifold)
{ {
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
} }
btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius ); 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); btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
if (!m_swapped) if (!m_swapped)
{ {
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
} else } else
{ {
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
} }
} }
}; };
}; };
#endif //SPHERE_BOX_COLLISION_ALGORITHM_H #endif //SPHERE_BOX_COLLISION_ALGORITHM_H

210
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@@ -1,105 +1,105 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btSphereSphereCollisionAlgorithm.h" #include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1) btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1)
: btActivatingCollisionAlgorithm(ci,col0,col1), : btActivatingCollisionAlgorithm(ci,col0,col1),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf) m_manifoldPtr(mf)
{ {
if (!m_manifoldPtr) if (!m_manifoldPtr)
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
m_ownManifold = true; m_ownManifold = true;
} }
} }
btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm() btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
} }
void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)dispatchInfo; (void)dispatchInfo;
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape(); btSphereShape* sphere0 = (btSphereShape*)col0->getCollisionShape();
btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape(); btSphereShape* sphere1 = (btSphereShape*)col1->getCollisionShape();
btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin(); btVector3 diff = col0->getWorldTransform().getOrigin()- col1->getWorldTransform().getOrigin();
btScalar len = diff.length(); btScalar len = diff.length();
btScalar radius0 = sphere0->getRadius(); btScalar radius0 = sphere0->getRadius();
btScalar radius1 = sphere1->getRadius(); btScalar radius1 = sphere1->getRadius();
#ifdef CLEAR_MANIFOLD #ifdef CLEAR_MANIFOLD
m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
#endif #endif
///iff distance positive, don't generate a new contact ///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1)) if ( len > (radius0+radius1))
{ {
#ifndef CLEAR_MANIFOLD #ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD #endif //CLEAR_MANIFOLD
return; return;
} }
///distance (negative means penetration) ///distance (negative means penetration)
btScalar dist = len - (radius0+radius1); btScalar dist = len - (radius0+radius1);
btVector3 normalOnSurfaceB(1,0,0); btVector3 normalOnSurfaceB(1,0,0);
if (len > SIMD_EPSILON) if (len > SIMD_EPSILON)
{ {
normalOnSurfaceB = diff / len; normalOnSurfaceB = diff / len;
} }
///point on A (worldspace) ///point on A (worldspace)
///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB; ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace) ///point on B (worldspace)
btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB; btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
#ifndef CLEAR_MANIFOLD #ifndef CLEAR_MANIFOLD
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
#endif //CLEAR_MANIFOLD #endif //CLEAR_MANIFOLD
} }
btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)col0; (void)col0;
(void)col1; (void)col1;
(void)dispatchInfo; (void)dispatchInfo;
(void)resultOut; (void)resultOut;
//not yet //not yet
return btScalar(1.); return btScalar(1.);
} }

132
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h
View File

@@ -1,66 +1,66 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H #ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
#define SPHERE_SPHERE_COLLISION_ALGORITHM_H #define SPHERE_SPHERE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h" #include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
class btPersistentManifold; class btPersistentManifold;
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response. /// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm /// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
public: public:
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {} : btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr && m_ownManifold) if (m_manifoldPtr && m_ownManifold)
{ {
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
} }
virtual ~btSphereSphereCollisionAlgorithm(); virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1); return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
} }
}; };
}; };
#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H #endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H

168
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp
View File

@@ -1,84 +1,84 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btSphereTriangleCollisionAlgorithm.h" #include "btSphereTriangleCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "SphereTriangleDetector.h" #include "SphereTriangleDetector.h"
btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped) btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
: btActivatingCollisionAlgorithm(ci,col0,col1), : btActivatingCollisionAlgorithm(ci,col0,col1),
m_ownManifold(false), m_ownManifold(false),
m_manifoldPtr(mf), m_manifoldPtr(mf),
m_swapped(swapped) m_swapped(swapped)
{ {
if (!m_manifoldPtr) if (!m_manifoldPtr)
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1); m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
m_ownManifold = true; m_ownManifold = true;
} }
} }
btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm() btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
{ {
if (m_ownManifold) if (m_ownManifold)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }
} }
void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
if (!m_manifoldPtr) if (!m_manifoldPtr)
return; return;
btCollisionObject* sphereObj = m_swapped? col1 : col0; btCollisionObject* sphereObj = m_swapped? col1 : col0;
btCollisionObject* triObj = m_swapped? col0 : col1; btCollisionObject* triObj = m_swapped? col0 : col1;
btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape(); btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape(); btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done /// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr); resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
btDiscreteCollisionDetectorInterface::ClosestPointInput input; btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds
input.m_transformA = sphereObj->getWorldTransform(); input.m_transformA = sphereObj->getWorldTransform();
input.m_transformB = triObj->getWorldTransform(); input.m_transformB = triObj->getWorldTransform();
bool swapResults = m_swapped; bool swapResults = m_swapped;
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults); detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
if (m_ownManifold) if (m_ownManifold)
resultOut->refreshContactPoints(); resultOut->refreshContactPoints();
} }
btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)resultOut; (void)resultOut;
(void)dispatchInfo; (void)dispatchInfo;
(void)col0; (void)col0;
(void)col1; (void)col1;
//not yet //not yet
return btScalar(1.); return btScalar(1.);
} }

138
src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h
View File

@@ -1,69 +1,69 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h" #include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold; class btPersistentManifold;
#include "btCollisionDispatcher.h" #include "btCollisionDispatcher.h"
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response. /// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm /// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
{ {
bool m_ownManifold; bool m_ownManifold;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
bool m_swapped; bool m_swapped;
public: public:
btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped); btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {} : btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr && m_ownManifold) if (m_manifoldPtr && m_ownManifold)
{ {
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
} }
virtual ~btSphereTriangleCollisionAlgorithm(); virtual ~btSphereTriangleCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
} }
}; };
}; };
#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H

342
src/BulletCollision/CollisionShapes/btCapsuleShape.cpp
View File

@@ -1,171 +1,171 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btCapsuleShape.h" #include "btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape () btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInternalShape ()
{ {
m_shapeType = CAPSULE_SHAPE_PROXYTYPE; m_shapeType = CAPSULE_SHAPE_PROXYTYPE;
m_upAxis = 1; m_upAxis = 1;
m_implicitShapeDimensions.setValue(radius,0.5f*height,radius); m_implicitShapeDimensions.setValue(radius,0.5f*height,radius);
} }
btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{ {
btVector3 supVec(0,0,0); btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30)); btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1,0,0);
} else } else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen; vec *= rlen;
} }
btVector3 vtx; btVector3 vtx;
btScalar newDot; btScalar newDot;
btScalar radius = getRadius(); btScalar radius = getRadius();
{ {
btVector3 pos(0,0,0); btVector3 pos(0,0,0);
pos[getUpAxis()] = getHalfHeight(); pos[getUpAxis()] = getHalfHeight();
vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
{ {
btVector3 pos(0,0,0); btVector3 pos(0,0,0);
pos[getUpAxis()] = -getHalfHeight(); pos[getUpAxis()] = -getHalfHeight();
vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
return supVec; return supVec;
} }
void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const void btCapsuleShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{ {
btScalar radius = getRadius(); btScalar radius = getRadius();
for (int j=0;j<numVectors;j++) for (int j=0;j<numVectors;j++)
{ {
btScalar maxDot(btScalar(-1e30)); btScalar maxDot(btScalar(-1e30));
const btVector3& vec = vectors[j]; const btVector3& vec = vectors[j];
btVector3 vtx; btVector3 vtx;
btScalar newDot; btScalar newDot;
{ {
btVector3 pos(0,0,0); btVector3 pos(0,0,0);
pos[getUpAxis()] = getHalfHeight(); pos[getUpAxis()] = getHalfHeight();
vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supportVerticesOut[j] = vtx; supportVerticesOut[j] = vtx;
} }
} }
{ {
btVector3 pos(0,0,0); btVector3 pos(0,0,0);
pos[getUpAxis()] = -getHalfHeight(); pos[getUpAxis()] = -getHalfHeight();
vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); vtx = pos +vec*m_localScaling*(radius) - vec * getMargin();
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supportVerticesOut[j] = vtx; supportVerticesOut[j] = vtx;
} }
} }
} }
} }
void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const void btCapsuleShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{ {
//as an approximation, take the inertia of the box that bounds the spheres //as an approximation, take the inertia of the box that bounds the spheres
btTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
btScalar radius = getRadius(); btScalar radius = getRadius();
btVector3 halfExtents(radius,radius,radius); btVector3 halfExtents(radius,radius,radius);
halfExtents[getUpAxis()]+=getHalfHeight(); halfExtents[getUpAxis()]+=getHalfHeight();
btScalar margin = CONVEX_DISTANCE_MARGIN; btScalar margin = CONVEX_DISTANCE_MARGIN;
btScalar lx=btScalar(2.)*(halfExtents[0]+margin); btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
btScalar ly=btScalar(2.)*(halfExtents[1]+margin); btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
btScalar lz=btScalar(2.)*(halfExtents[2]+margin); btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
const btScalar x2 = lx*lx; const btScalar x2 = lx*lx;
const btScalar y2 = ly*ly; const btScalar y2 = ly*ly;
const btScalar z2 = lz*lz; const btScalar z2 = lz*lz;
const btScalar scaledmass = mass * btScalar(.08333333); const btScalar scaledmass = mass * btScalar(.08333333);
inertia[0] = scaledmass * (y2+z2); inertia[0] = scaledmass * (y2+z2);
inertia[1] = scaledmass * (x2+z2); inertia[1] = scaledmass * (x2+z2);
inertia[2] = scaledmass * (x2+y2); inertia[2] = scaledmass * (x2+y2);
} }
btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height) btCapsuleShapeX::btCapsuleShapeX(btScalar radius,btScalar height)
{ {
m_upAxis = 0; m_upAxis = 0;
m_implicitShapeDimensions.setValue(0.5f*height, radius,radius); m_implicitShapeDimensions.setValue(0.5f*height, radius,radius);
} }
btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height) btCapsuleShapeZ::btCapsuleShapeZ(btScalar radius,btScalar height)
{ {
m_upAxis = 2; m_upAxis = 2;
m_implicitShapeDimensions.setValue(radius,radius,0.5f*height); m_implicitShapeDimensions.setValue(radius,radius,0.5f*height);
} }

236
src/BulletCollision/CollisionShapes/btCapsuleShape.h
View File

@@ -1,118 +1,118 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_CAPSULE_SHAPE_H #ifndef BT_CAPSULE_SHAPE_H
#define BT_CAPSULE_SHAPE_H #define BT_CAPSULE_SHAPE_H
#include "btConvexInternalShape.h" #include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #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 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 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. ///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 class btCapsuleShape : public btConvexInternalShape
{ {
protected: protected:
int m_upAxis; int m_upAxis;
protected: protected:
///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses. ///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;}; btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
public: public:
btCapsuleShape(btScalar radius,btScalar height); btCapsuleShape(btScalar radius,btScalar height);
///CollisionShape Interface ///CollisionShape Interface
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
/// btConvexShape Interface /// btConvexShape Interface
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{ {
btVector3 halfExtents(getRadius(),getRadius(),getRadius()); btVector3 halfExtents(getRadius(),getRadius(),getRadius());
halfExtents[m_upAxis] = getRadius() + getHalfHeight(); halfExtents[m_upAxis] = getRadius() + getHalfHeight();
halfExtents += btVector3(getMargin(),getMargin(),getMargin()); halfExtents += btVector3(getMargin(),getMargin(),getMargin());
btMatrix3x3 abs_b = t.getBasis().absolute(); btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin(); btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents)); btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
aabbMin = center - extent; aabbMin = center - extent;
aabbMax = center + extent; aabbMax = center + extent;
} }
virtual const char* getName()const virtual const char* getName()const
{ {
return "CapsuleShape"; return "CapsuleShape";
} }
int getUpAxis() const int getUpAxis() const
{ {
return m_upAxis; return m_upAxis;
} }
btScalar getRadius() const btScalar getRadius() const
{ {
int radiusAxis = (m_upAxis+2)%3; int radiusAxis = (m_upAxis+2)%3;
return m_implicitShapeDimensions[radiusAxis]; return m_implicitShapeDimensions[radiusAxis];
} }
btScalar getHalfHeight() const btScalar getHalfHeight() const
{ {
return m_implicitShapeDimensions[m_upAxis]; return m_implicitShapeDimensions[m_upAxis];
} }
}; };
///btCapsuleShapeX represents a capsule around the Z axis ///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. ///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 class btCapsuleShapeX : public btCapsuleShape
{ {
public: public:
btCapsuleShapeX(btScalar radius,btScalar height); btCapsuleShapeX(btScalar radius,btScalar height);
//debugging //debugging
virtual const char* getName()const virtual const char* getName()const
{ {
return "CapsuleX"; return "CapsuleX";
} }
}; };
///btCapsuleShapeZ represents a capsule around the Z axis ///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. ///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 class btCapsuleShapeZ : public btCapsuleShape
{ {
public: public:
btCapsuleShapeZ(btScalar radius,btScalar height); btCapsuleShapeZ(btScalar radius,btScalar height);
//debugging //debugging
virtual const char* getName()const virtual const char* getName()const
{ {
return "CapsuleZ"; return "CapsuleZ";
} }
}; };
#endif //BT_CAPSULE_SHAPE_H #endif //BT_CAPSULE_SHAPE_H

312
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
View File

@@ -1,156 +1,156 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btConvexPointCloudShape.h" #include "btConvexPointCloudShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling) void btConvexPointCloudShape::setLocalScaling(const btVector3& scaling)
{ {
m_localScaling = scaling; m_localScaling = scaling;
recalcLocalAabb(); recalcLocalAabb();
} }
#ifndef __SPU__ #ifndef __SPU__
btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{ {
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-1e30); btScalar newDot,maxDot = btScalar(-1e30);
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1,0,0);
} else } else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen; vec *= rlen;
} }
for (int i=0;i<m_numPoints;i++) for (int i=0;i<m_numPoints;i++)
{ {
btVector3 vtx = getScaledPoint(i); btVector3 vtx = getScaledPoint(i);
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > maxDot) if (newDot > maxDot)
{ {
maxDot = newDot; maxDot = newDot;
supVec = vtx; supVec = vtx;
} }
} }
return supVec; return supVec;
} }
void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const void btConvexPointCloudShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{ {
btScalar newDot; btScalar newDot;
//use 'w' component of supportVerticesOut? //use 'w' component of supportVerticesOut?
{ {
for (int i=0;i<numVectors;i++) for (int i=0;i<numVectors;i++)
{ {
supportVerticesOut[i][3] = btScalar(-1e30); supportVerticesOut[i][3] = btScalar(-1e30);
} }
} }
for (int i=0;i<m_numPoints;i++) for (int i=0;i<m_numPoints;i++)
{ {
btVector3 vtx = getScaledPoint(i); btVector3 vtx = getScaledPoint(i);
for (int j=0;j<numVectors;j++) for (int j=0;j<numVectors;j++)
{ {
const btVector3& vec = vectors[j]; const btVector3& vec = vectors[j];
newDot = vec.dot(vtx); newDot = vec.dot(vtx);
if (newDot > supportVerticesOut[j][3]) if (newDot > supportVerticesOut[j][3])
{ {
//WARNING: don't swap next lines, the w component would get overwritten! //WARNING: don't swap next lines, the w component would get overwritten!
supportVerticesOut[j] = vtx; supportVerticesOut[j] = vtx;
supportVerticesOut[j][3] = newDot; supportVerticesOut[j][3] = newDot;
} }
} }
} }
} }
btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const btVector3 btConvexPointCloudShape::localGetSupportingVertex(const btVector3& vec)const
{ {
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
if ( getMargin()!=btScalar(0.) ) if ( getMargin()!=btScalar(0.) )
{ {
btVector3 vecnorm = vec; btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
{ {
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
} }
vecnorm.normalize(); vecnorm.normalize();
supVertex+= getMargin() * vecnorm; supVertex+= getMargin() * vecnorm;
} }
return supVertex; return supVertex;
} }
#endif #endif
//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //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 //Please note that you can debug-draw btConvexHullShape with the Raytracer Demo
int btConvexPointCloudShape::getNumVertices() const int btConvexPointCloudShape::getNumVertices() const
{ {
return m_numPoints; return m_numPoints;
} }
int btConvexPointCloudShape::getNumEdges() const int btConvexPointCloudShape::getNumEdges() const
{ {
return 0; return 0;
} }
void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const void btConvexPointCloudShape::getEdge(int i,btVector3& pa,btVector3& pb) const
{ {
btAssert (0); btAssert (0);
} }
void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const void btConvexPointCloudShape::getVertex(int i,btVector3& vtx) const
{ {
vtx = m_unscaledPoints[i]*m_localScaling; vtx = m_unscaledPoints[i]*m_localScaling;
} }
int btConvexPointCloudShape::getNumPlanes() const int btConvexPointCloudShape::getNumPlanes() const
{ {
return 0; return 0;
} }
void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const void btConvexPointCloudShape::getPlane(btVector3& ,btVector3& ,int ) const
{ {
btAssert(0); btAssert(0);
} }
//not yet //not yet
bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
{ {
btAssert(0); btAssert(0);
return false; return false;
} }

192
src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h
View File

@@ -1,96 +1,96 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H #ifndef BT_CONVEX_POINT_CLOUD_SHAPE_H
#define BT_CONVEX_POINT_CLOUD_SHAPE_H #define BT_CONVEX_POINT_CLOUD_SHAPE_H
#include "btPolyhedralConvexShape.h" #include "btPolyhedralConvexShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices. ///The btConvexPointCloudShape implements an implicit convex hull of an array of vertices.
ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape ATTRIBUTE_ALIGNED16(class) btConvexPointCloudShape : public btPolyhedralConvexShape
{ {
btVector3* m_unscaledPoints; btVector3* m_unscaledPoints;
int m_numPoints; int m_numPoints;
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true) btConvexPointCloudShape(btVector3* points,int numPoints, const btVector3& localScaling,bool computeAabb = true)
{ {
m_localScaling = localScaling; m_localScaling = localScaling;
m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE; m_shapeType = CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE;
m_unscaledPoints = points; m_unscaledPoints = points;
m_numPoints = numPoints; m_numPoints = numPoints;
if (computeAabb) if (computeAabb)
recalcLocalAabb(); recalcLocalAabb();
} }
void setPoints (btVector3* points, int numPoints, bool computeAabb = true) void setPoints (btVector3* points, int numPoints, bool computeAabb = true)
{ {
m_unscaledPoints = points; m_unscaledPoints = points;
m_numPoints = numPoints; m_numPoints = numPoints;
if (computeAabb) if (computeAabb)
recalcLocalAabb(); recalcLocalAabb();
} }
SIMD_FORCE_INLINE btVector3* getUnscaledPoints() SIMD_FORCE_INLINE btVector3* getUnscaledPoints()
{ {
return m_unscaledPoints; return m_unscaledPoints;
} }
SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const SIMD_FORCE_INLINE const btVector3* getUnscaledPoints() const
{ {
return m_unscaledPoints; return m_unscaledPoints;
} }
SIMD_FORCE_INLINE int getNumPoints() const SIMD_FORCE_INLINE int getNumPoints() const
{ {
return m_numPoints; return m_numPoints;
} }
SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const SIMD_FORCE_INLINE btVector3 getScaledPoint( int index) const
{ {
return m_unscaledPoints[index] * m_localScaling; return m_unscaledPoints[index] * m_localScaling;
} }
#ifndef __SPU__ #ifndef __SPU__
virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; virtual btVector3 localGetSupportingVertex(const btVector3& vec)const;
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
#endif #endif
//debugging //debugging
virtual const char* getName()const {return "ConvexPointCloud";} virtual const char* getName()const {return "ConvexPointCloud";}
virtual int getNumVertices() const; virtual int getNumVertices() const;
virtual int getNumEdges() const; virtual int getNumEdges() const;
virtual void getEdge(int i,btVector3& pa,btVector3& pb) const; virtual void getEdge(int i,btVector3& pa,btVector3& pb) const;
virtual void getVertex(int i,btVector3& vtx) const; virtual void getVertex(int i,btVector3& vtx) const;
virtual int getNumPlanes() const; virtual int getNumPlanes() const;
virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const; virtual void getPlane(btVector3& planeNormal,btVector3& planeSupport,int i ) const;
virtual bool isInside(const btVector3& pt,btScalar tolerance) const; virtual bool isInside(const btVector3& pt,btScalar tolerance) const;
///in case we receive negative scaling ///in case we receive negative scaling
virtual void setLocalScaling(const btVector3& scaling); virtual void setLocalScaling(const btVector3& scaling);
}; };
#endif //BT_CONVEX_POINT_CLOUD_SHAPE_H #endif //BT_CONVEX_POINT_CLOUD_SHAPE_H

628
src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp
View File

@@ -1,314 +1,314 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btConvexTriangleMeshShape.h" #include "btConvexTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btCollisionMargin.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb) btConvexTriangleMeshShape ::btConvexTriangleMeshShape (btStridingMeshInterface* meshInterface, bool calcAabb)
: btPolyhedralConvexShape(), m_stridingMesh(meshInterface) : btPolyhedralConvexShape(), m_stridingMesh(meshInterface)
{ {
m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; m_shapeType = CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE;
if ( calcAabb ) if ( calcAabb )
recalcLocalAabb(); recalcLocalAabb();
} }
///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once ///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 ///but then we are duplicating
class LocalSupportVertexCallback: public btInternalTriangleIndexCallback class LocalSupportVertexCallback: public btInternalTriangleIndexCallback
{ {
btVector3 m_supportVertexLocal; btVector3 m_supportVertexLocal;
public: public:
btScalar m_maxDot; btScalar m_maxDot;
btVector3 m_supportVecLocal; btVector3 m_supportVecLocal;
LocalSupportVertexCallback(const btVector3& supportVecLocal) LocalSupportVertexCallback(const btVector3& supportVecLocal)
: m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)), : m_supportVertexLocal(btScalar(0.),btScalar(0.),btScalar(0.)),
m_maxDot(btScalar(-1e30)), m_maxDot(btScalar(-1e30)),
m_supportVecLocal(supportVecLocal) m_supportVecLocal(supportVecLocal)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex)
{ {
(void)triangleIndex; (void)triangleIndex;
(void)partId; (void)partId;
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
btScalar dot = m_supportVecLocal.dot(triangle[i]); btScalar dot = m_supportVecLocal.dot(triangle[i]);
if (dot > m_maxDot) if (dot > m_maxDot)
{ {
m_maxDot = dot; m_maxDot = dot;
m_supportVertexLocal = triangle[i]; m_supportVertexLocal = triangle[i];
} }
} }
} }
btVector3 GetSupportVertexLocal() btVector3 GetSupportVertexLocal()
{ {
return m_supportVertexLocal; return m_supportVertexLocal;
} }
}; };
btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{ {
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.)); btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec = vec0; btVector3 vec = vec0;
btScalar lenSqr = vec.length2(); btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001)) if (lenSqr < btScalar(0.0001))
{ {
vec.setValue(1,0,0); vec.setValue(1,0,0);
} else } else
{ {
btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen; vec *= rlen;
} }
LocalSupportVertexCallback supportCallback(vec); LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal(); supVec = supportCallback.GetSupportVertexLocal();
return supVec; return supVec;
} }
void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const void btConvexTriangleMeshShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
{ {
//use 'w' component of supportVerticesOut? //use 'w' component of supportVerticesOut?
{ {
for (int i=0;i<numVectors;i++) for (int i=0;i<numVectors;i++)
{ {
supportVerticesOut[i][3] = btScalar(-1e30); supportVerticesOut[i][3] = btScalar(-1e30);
} }
} }
///@todo: could do the batch inside the callback! ///@todo: could do the batch inside the callback!
for (int j=0;j<numVectors;j++) for (int j=0;j<numVectors;j++)
{ {
const btVector3& vec = vectors[j]; const btVector3& vec = vectors[j];
LocalSupportVertexCallback supportCallback(vec); LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
} }
} }
btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const
{ {
btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec);
if ( getMargin()!=btScalar(0.) ) if ( getMargin()!=btScalar(0.) )
{ {
btVector3 vecnorm = vec; btVector3 vecnorm = vec;
if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
{ {
vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.)); vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
} }
vecnorm.normalize(); vecnorm.normalize();
supVertex+= getMargin() * vecnorm; supVertex+= getMargin() * vecnorm;
} }
return supVertex; return supVertex;
} }
//currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection //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 //Please note that you can debug-draw btConvexTriangleMeshShape with the Raytracer Demo
int btConvexTriangleMeshShape::getNumVertices() const int btConvexTriangleMeshShape::getNumVertices() const
{ {
//cache this? //cache this?
return 0; return 0;
} }
int btConvexTriangleMeshShape::getNumEdges() const int btConvexTriangleMeshShape::getNumEdges() const
{ {
return 0; return 0;
} }
void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const void btConvexTriangleMeshShape::getEdge(int ,btVector3& ,btVector3& ) const
{ {
btAssert(0); btAssert(0);
} }
void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const void btConvexTriangleMeshShape::getVertex(int ,btVector3& ) const
{ {
btAssert(0); btAssert(0);
} }
int btConvexTriangleMeshShape::getNumPlanes() const int btConvexTriangleMeshShape::getNumPlanes() const
{ {
return 0; return 0;
} }
void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const void btConvexTriangleMeshShape::getPlane(btVector3& ,btVector3& ,int ) const
{ {
btAssert(0); btAssert(0);
} }
//not yet //not yet
bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const bool btConvexTriangleMeshShape::isInside(const btVector3& ,btScalar ) const
{ {
btAssert(0); btAssert(0);
return false; return false;
} }
void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling)
{ {
m_stridingMesh->setScaling(scaling); m_stridingMesh->setScaling(scaling);
recalcLocalAabb(); recalcLocalAabb();
} }
const btVector3& btConvexTriangleMeshShape::getLocalScaling() const const btVector3& btConvexTriangleMeshShape::getLocalScaling() const
{ {
return m_stridingMesh->getScaling(); return m_stridingMesh->getScaling();
} }
void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const void btConvexTriangleMeshShape::calculatePrincipalAxisTransform(btTransform& principal, btVector3& inertia, btScalar& volume) const
{ {
class CenterCallback: public btInternalTriangleIndexCallback class CenterCallback: public btInternalTriangleIndexCallback
{ {
bool first; bool first;
btVector3 ref; btVector3 ref;
btVector3 sum; btVector3 sum;
btScalar volume; btScalar volume;
public: public:
CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0) CenterCallback() : first(true), ref(0, 0, 0), sum(0, 0, 0), volume(0)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{ {
(void) triangleIndex; (void) triangleIndex;
(void) partId; (void) partId;
if (first) if (first)
{ {
ref = triangle[0]; ref = triangle[0];
first = false; first = false;
} }
else else
{ {
btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref)); 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)); sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
volume += vol; volume += vol;
} }
} }
btVector3 getCenter() btVector3 getCenter()
{ {
return (volume > 0) ? sum / volume : ref; return (volume > 0) ? sum / volume : ref;
} }
btScalar getVolume() btScalar getVolume()
{ {
return volume * btScalar(1. / 6); return volume * btScalar(1. / 6);
} }
}; };
class InertiaCallback: public btInternalTriangleIndexCallback class InertiaCallback: public btInternalTriangleIndexCallback
{ {
btMatrix3x3 sum; btMatrix3x3 sum;
btVector3 center; btVector3 center;
public: public:
InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center) InertiaCallback(btVector3& center) : sum(0, 0, 0, 0, 0, 0, 0, 0, 0), center(center)
{ {
} }
virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex) virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
{ {
(void) triangleIndex; (void) triangleIndex;
(void) partId; (void) partId;
btMatrix3x3 i; btMatrix3x3 i;
btVector3 a = triangle[0] - center; btVector3 a = triangle[0] - center;
btVector3 b = triangle[1] - center; btVector3 b = triangle[1] - center;
btVector3 c = triangle[2] - center; btVector3 c = triangle[2] - center;
btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6); btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
for (int j = 0; j < 3; j++) for (int j = 0; j < 3; j++)
{ {
for (int k = 0; k <= j; k++) 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]) 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(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 i00 = -i[0][0];
btScalar i11 = -i[1][1]; btScalar i11 = -i[1][1];
btScalar i22 = -i[2][2]; btScalar i22 = -i[2][2];
i[0][0] = i11 + i22; i[0][0] = i11 + i22;
i[1][1] = i22 + i00; i[1][1] = i22 + i00;
i[2][2] = i00 + i11; i[2][2] = i00 + i11;
sum[0] += i[0]; sum[0] += i[0];
sum[1] += i[1]; sum[1] += i[1];
sum[2] += i[2]; sum[2] += i[2];
} }
btMatrix3x3& getInertia() btMatrix3x3& getInertia()
{ {
return sum; return sum;
} }
}; };
CenterCallback centerCallback; CenterCallback centerCallback;
btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax); m_stridingMesh->InternalProcessAllTriangles(&centerCallback, -aabbMax, aabbMax);
btVector3 center = centerCallback.getCenter(); btVector3 center = centerCallback.getCenter();
principal.setOrigin(center); principal.setOrigin(center);
volume = centerCallback.getVolume(); volume = centerCallback.getVolume();
InertiaCallback inertiaCallback(center); InertiaCallback inertiaCallback(center);
m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax); m_stridingMesh->InternalProcessAllTriangles(&inertiaCallback, -aabbMax, aabbMax);
btMatrix3x3& i = inertiaCallback.getInertia(); btMatrix3x3& i = inertiaCallback.getInertia();
i.diagonalize(principal.getBasis(), btScalar(0.00001), 20); i.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(i[0][0], i[1][1], i[2][2]); inertia.setValue(i[0][0], i[1][1], i[2][2]);
inertia /= volume; inertia /= volume;
} }

822
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
View File

@@ -1,411 +1,411 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btHeightfieldTerrainShape.h" #include "btHeightfieldTerrainShape.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
btHeightfieldTerrainShape::btHeightfieldTerrainShape btHeightfieldTerrainShape::btHeightfieldTerrainShape
( (
int heightStickWidth, int heightStickLength, void* heightfieldData, int heightStickWidth, int heightStickLength, void* heightfieldData,
btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis,
PHY_ScalarType hdt, bool flipQuadEdges PHY_ScalarType hdt, bool flipQuadEdges
) )
{ {
initialize(heightStickWidth, heightStickLength, heightfieldData, initialize(heightStickWidth, heightStickLength, heightfieldData,
heightScale, minHeight, maxHeight, upAxis, hdt, heightScale, minHeight, maxHeight, upAxis, hdt,
flipQuadEdges); flipQuadEdges);
} }
btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool 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, // legacy constructor: support only float or unsigned char,
// and min height is zero // and min height is zero
PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR; PHY_ScalarType hdt = (useFloatData) ? PHY_FLOAT : PHY_UCHAR;
btScalar minHeight = 0.0; btScalar minHeight = 0.0;
// previously, height = uchar * maxHeight / 65535. // previously, height = uchar * maxHeight / 65535.
// So to preserve legacy behavior, heightScale = maxHeight / 65535 // So to preserve legacy behavior, heightScale = maxHeight / 65535
btScalar heightScale = maxHeight / 65535; btScalar heightScale = maxHeight / 65535;
initialize(heightStickWidth, heightStickLength, heightfieldData, initialize(heightStickWidth, heightStickLength, heightfieldData,
heightScale, minHeight, maxHeight, upAxis, hdt, heightScale, minHeight, maxHeight, upAxis, hdt,
flipQuadEdges); flipQuadEdges);
} }
void btHeightfieldTerrainShape::initialize void btHeightfieldTerrainShape::initialize
( (
int heightStickWidth, int heightStickLength, void* heightfieldData, int heightStickWidth, int heightStickLength, void* heightfieldData,
btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis,
PHY_ScalarType hdt, bool flipQuadEdges PHY_ScalarType hdt, bool flipQuadEdges
) )
{ {
// validation // validation
btAssert(heightStickWidth > 1 && "bad width"); btAssert(heightStickWidth > 1 && "bad width");
btAssert(heightStickLength > 1 && "bad length"); btAssert(heightStickLength > 1 && "bad length");
btAssert(heightfieldData && "null heightfield data"); btAssert(heightfieldData && "null heightfield data");
// btAssert(heightScale) -- do we care? Trust caller here // btAssert(heightScale) -- do we care? Trust caller here
btAssert(minHeight <= maxHeight && "bad min/max height"); btAssert(minHeight <= maxHeight && "bad min/max height");
btAssert(upAxis >= 0 && upAxis < 3 && btAssert(upAxis >= 0 && upAxis < 3 &&
"bad upAxis--should be in range [0,2]"); "bad upAxis--should be in range [0,2]");
btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT && btAssert(hdt != PHY_UCHAR || hdt != PHY_FLOAT || hdt != PHY_SHORT &&
"Bad height data type enum"); "Bad height data type enum");
// initialize member variables // initialize member variables
m_shapeType = TERRAIN_SHAPE_PROXYTYPE; m_shapeType = TERRAIN_SHAPE_PROXYTYPE;
m_heightStickWidth = heightStickWidth; m_heightStickWidth = heightStickWidth;
m_heightStickLength = heightStickLength; m_heightStickLength = heightStickLength;
m_minHeight = minHeight; m_minHeight = minHeight;
m_maxHeight = maxHeight; m_maxHeight = maxHeight;
m_width = (btScalar) (heightStickWidth - 1); m_width = (btScalar) (heightStickWidth - 1);
m_length = (btScalar) (heightStickLength - 1); m_length = (btScalar) (heightStickLength - 1);
m_heightScale = heightScale; m_heightScale = heightScale;
m_heightfieldDataUnknown = heightfieldData; m_heightfieldDataUnknown = heightfieldData;
m_heightDataType = hdt; m_heightDataType = hdt;
m_flipQuadEdges = flipQuadEdges; m_flipQuadEdges = flipQuadEdges;
m_useDiamondSubdivision = false; m_useDiamondSubdivision = false;
m_upAxis = upAxis; m_upAxis = upAxis;
m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.)); m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
// determine min/max axis-aligned bounding box (aabb) values // determine min/max axis-aligned bounding box (aabb) values
switch (m_upAxis) switch (m_upAxis)
{ {
case 0: case 0:
{ {
m_localAabbMin.setValue(m_minHeight, 0, 0); m_localAabbMin.setValue(m_minHeight, 0, 0);
m_localAabbMax.setValue(m_maxHeight, m_width, m_length); m_localAabbMax.setValue(m_maxHeight, m_width, m_length);
break; break;
} }
case 1: case 1:
{ {
m_localAabbMin.setValue(0, m_minHeight, 0); m_localAabbMin.setValue(0, m_minHeight, 0);
m_localAabbMax.setValue(m_width, m_maxHeight, m_length); m_localAabbMax.setValue(m_width, m_maxHeight, m_length);
break; break;
}; };
case 2: case 2:
{ {
m_localAabbMin.setValue(0, 0, m_minHeight); m_localAabbMin.setValue(0, 0, m_minHeight);
m_localAabbMax.setValue(m_width, m_length, m_maxHeight); m_localAabbMax.setValue(m_width, m_length, m_maxHeight);
break; break;
} }
default: default:
{ {
//need to get valid m_upAxis //need to get valid m_upAxis
btAssert(0 && "Bad m_upAxis"); btAssert(0 && "Bad m_upAxis");
} }
} }
// remember origin (defined as exact middle of aabb) // remember origin (defined as exact middle of aabb)
m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax); m_localOrigin = btScalar(0.5) * (m_localAabbMin + m_localAabbMax);
} }
btHeightfieldTerrainShape::~btHeightfieldTerrainShape() btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
{ {
} }
void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{ {
btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
btVector3 localOrigin(0, 0, 0); btVector3 localOrigin(0, 0, 0);
localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5); localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
localOrigin *= m_localScaling; localOrigin *= m_localScaling;
btMatrix3x3 abs_b = t.getBasis().absolute(); btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin(); btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents), btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
abs_b[1].dot(halfExtents), abs_b[1].dot(halfExtents),
abs_b[2].dot(halfExtents)); abs_b[2].dot(halfExtents));
extent += btVector3(getMargin(),getMargin(),getMargin()); extent += btVector3(getMargin(),getMargin(),getMargin());
aabbMin = center - extent; aabbMin = center - extent;
aabbMax = center + extent; aabbMax = center + extent;
} }
/// This returns the "raw" (user's initial) height, not the actual height. /// 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 /// The actual height needs to be adjusted to be relative to the center
/// of the heightfield's AABB. /// of the heightfield's AABB.
btScalar btScalar
btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
{ {
btScalar val = 0.f; btScalar val = 0.f;
switch (m_heightDataType) switch (m_heightDataType)
{ {
case PHY_FLOAT: case PHY_FLOAT:
{ {
val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x]; val = m_heightfieldDataFloat[(y*m_heightStickWidth)+x];
break; break;
} }
case PHY_UCHAR: case PHY_UCHAR:
{ {
unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x]; unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_heightStickWidth)+x];
val = heightFieldValue * m_heightScale; val = heightFieldValue * m_heightScale;
break; break;
} }
case PHY_SHORT: case PHY_SHORT:
{ {
short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x]; short hfValue = m_heightfieldDataShort[(y * m_heightStickWidth) + x];
val = hfValue * m_heightScale; val = hfValue * m_heightScale;
break; break;
} }
default: default:
{ {
btAssert(!"Bad m_heightDataType"); btAssert(!"Bad m_heightDataType");
} }
} }
return val; return val;
} }
/// this returns the vertex in bullet-local coordinates /// this returns the vertex in bullet-local coordinates
void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
{ {
btAssert(x>=0); btAssert(x>=0);
btAssert(y>=0); btAssert(y>=0);
btAssert(x<m_heightStickWidth); btAssert(x<m_heightStickWidth);
btAssert(y<m_heightStickLength); btAssert(y<m_heightStickLength);
btScalar height = getRawHeightFieldValue(x,y); btScalar height = getRawHeightFieldValue(x,y);
switch (m_upAxis) switch (m_upAxis)
{ {
case 0: case 0:
{ {
vertex.setValue( vertex.setValue(
height - m_localOrigin.getX(), height - m_localOrigin.getX(),
(-m_width/btScalar(2.0)) + x, (-m_width/btScalar(2.0)) + x,
(-m_length/btScalar(2.0) ) + y (-m_length/btScalar(2.0) ) + y
); );
break; break;
} }
case 1: case 1:
{ {
vertex.setValue( vertex.setValue(
(-m_width/btScalar(2.0)) + x, (-m_width/btScalar(2.0)) + x,
height - m_localOrigin.getY(), height - m_localOrigin.getY(),
(-m_length/btScalar(2.0)) + y (-m_length/btScalar(2.0)) + y
); );
break; break;
}; };
case 2: case 2:
{ {
vertex.setValue( vertex.setValue(
(-m_width/btScalar(2.0)) + x, (-m_width/btScalar(2.0)) + x,
(-m_length/btScalar(2.0)) + y, (-m_length/btScalar(2.0)) + y,
height - m_localOrigin.getZ() height - m_localOrigin.getZ()
); );
break; break;
} }
default: default:
{ {
//need to get valid m_upAxis //need to get valid m_upAxis
btAssert(0); btAssert(0);
} }
} }
vertex*=m_localScaling; vertex*=m_localScaling;
} }
static inline int static inline int
getQuantized getQuantized
( (
btScalar x btScalar x
) )
{ {
if (x < 0.0) { if (x < 0.0) {
return (int) (x - 0.5); return (int) (x - 0.5);
} }
return (int) (x + 0.5); return (int) (x + 0.5);
} }
/// given input vector, return quantized version /// given input vector, return quantized version
/** /**
This routine is basically determining the gridpoint indices for a given This routine is basically determining the gridpoint indices for a given
input vector, answering the question: "which gridpoint is closest to the input vector, answering the question: "which gridpoint is closest to the
provided point?". provided point?".
"with clamp" means that we restrict the point to be in the heightfield's "with clamp" means that we restrict the point to be in the heightfield's
axis-aligned bounding box. axis-aligned bounding box.
*/ */
void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
{ {
btVector3 clampedPoint(point); btVector3 clampedPoint(point);
clampedPoint.setMax(m_localAabbMin); clampedPoint.setMax(m_localAabbMin);
clampedPoint.setMin(m_localAabbMax); clampedPoint.setMin(m_localAabbMax);
out[0] = getQuantized(clampedPoint.getX()); out[0] = getQuantized(clampedPoint.getX());
out[1] = getQuantized(clampedPoint.getY()); out[1] = getQuantized(clampedPoint.getY());
out[2] = getQuantized(clampedPoint.getZ()); out[2] = getQuantized(clampedPoint.getZ());
} }
/// process all triangles within the provided axis-aligned bounding box /// process all triangles within the provided axis-aligned bounding box
/** /**
basic algorithm: basic algorithm:
- convert input aabb to local coordinates (scale down and shift for local origin) - convert input aabb to local coordinates (scale down and shift for local origin)
- convert input aabb to a range of heightfield grid points (quantize) - convert input aabb to a range of heightfield grid points (quantize)
- iterate over all triangles in that subset of the grid - iterate over all triangles in that subset of the grid
*/ */
void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{ {
// scale down the input aabb's so they are in local (non-scaled) coordinates // scale down the input aabb's so they are in local (non-scaled) coordinates
btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); btVector3 localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]); btVector3 localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
// account for local origin // account for local origin
localAabbMin += m_localOrigin; localAabbMin += m_localOrigin;
localAabbMax += m_localOrigin; localAabbMax += m_localOrigin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges //quantize the aabbMin and aabbMax, and adjust the start/end ranges
int quantizedAabbMin[3]; int quantizedAabbMin[3];
int quantizedAabbMax[3]; int quantizedAabbMax[3];
quantizeWithClamp(quantizedAabbMin, localAabbMin,0); quantizeWithClamp(quantizedAabbMin, localAabbMin,0);
quantizeWithClamp(quantizedAabbMax, localAabbMax,1); quantizeWithClamp(quantizedAabbMax, localAabbMax,1);
// expand the min/max quantized values // expand the min/max quantized values
// this is to catch the case where the input aabb falls between grid points! // this is to catch the case where the input aabb falls between grid points!
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
quantizedAabbMin[i]--; quantizedAabbMin[i]--;
quantizedAabbMax[i]++; quantizedAabbMax[i]++;
} }
int startX=0; int startX=0;
int endX=m_heightStickWidth-1; int endX=m_heightStickWidth-1;
int startJ=0; int startJ=0;
int endJ=m_heightStickLength-1; int endJ=m_heightStickLength-1;
switch (m_upAxis) switch (m_upAxis)
{ {
case 0: case 0:
{ {
if (quantizedAabbMin[1]>startX) if (quantizedAabbMin[1]>startX)
startX = quantizedAabbMin[1]; startX = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endX) if (quantizedAabbMax[1]<endX)
endX = quantizedAabbMax[1]; endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ) if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2]; startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ) if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2]; endJ = quantizedAabbMax[2];
break; break;
} }
case 1: case 1:
{ {
if (quantizedAabbMin[0]>startX) if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0]; startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX) if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0]; endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ) if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2]; startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ) if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2]; endJ = quantizedAabbMax[2];
break; break;
}; };
case 2: case 2:
{ {
if (quantizedAabbMin[0]>startX) if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0]; startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX) if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0]; endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ) if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1]; startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ) if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1]; endJ = quantizedAabbMax[1];
break; break;
} }
default: default:
{ {
//need to get valid m_upAxis //need to get valid m_upAxis
btAssert(0); btAssert(0);
} }
} }
for(int j=startJ; j<endJ; j++) for(int j=startJ; j<endJ; j++)
{ {
for(int x=startX; x<endX; x++) for(int x=startX; x<endX; x++)
{ {
btVector3 vertices[3]; btVector3 vertices[3];
if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1))) if (m_flipQuadEdges || (m_useDiamondSubdivision && !((j+x) & 1)))
{ {
//first triangle //first triangle
getVertex(x,j,vertices[0]); getVertex(x,j,vertices[0]);
getVertex(x+1,j,vertices[1]); getVertex(x+1,j,vertices[1]);
getVertex(x+1,j+1,vertices[2]); getVertex(x+1,j+1,vertices[2]);
callback->processTriangle(vertices,x,j); callback->processTriangle(vertices,x,j);
//second triangle //second triangle
getVertex(x,j,vertices[0]); getVertex(x,j,vertices[0]);
getVertex(x+1,j+1,vertices[1]); getVertex(x+1,j+1,vertices[1]);
getVertex(x,j+1,vertices[2]); getVertex(x,j+1,vertices[2]);
callback->processTriangle(vertices,x,j); callback->processTriangle(vertices,x,j);
} else } else
{ {
//first triangle //first triangle
getVertex(x,j,vertices[0]); getVertex(x,j,vertices[0]);
getVertex(x,j+1,vertices[1]); getVertex(x,j+1,vertices[1]);
getVertex(x+1,j,vertices[2]); getVertex(x+1,j,vertices[2]);
callback->processTriangle(vertices,x,j); callback->processTriangle(vertices,x,j);
//second triangle //second triangle
getVertex(x+1,j,vertices[0]); getVertex(x+1,j,vertices[0]);
getVertex(x,j+1,vertices[1]); getVertex(x,j+1,vertices[1]);
getVertex(x+1,j+1,vertices[2]); getVertex(x+1,j+1,vertices[2]);
callback->processTriangle(vertices,x,j); callback->processTriangle(vertices,x,j);
} }
} }
} }
} }
void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const void btHeightfieldTerrainShape::calculateLocalInertia(btScalar ,btVector3& inertia) const
{ {
//moving concave objects not supported //moving concave objects not supported
inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
} }
void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling) void btHeightfieldTerrainShape::setLocalScaling(const btVector3& scaling)
{ {
m_localScaling = scaling; m_localScaling = scaling;
} }
const btVector3& btHeightfieldTerrainShape::getLocalScaling() const const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
{ {
return m_localScaling; return m_localScaling;
} }

322
src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
View File

@@ -1,161 +1,161 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H #ifndef HEIGHTFIELD_TERRAIN_SHAPE_H
#define HEIGHTFIELD_TERRAIN_SHAPE_H #define HEIGHTFIELD_TERRAIN_SHAPE_H
#include "btConcaveShape.h" #include "btConcaveShape.h"
///btHeightfieldTerrainShape simulates a 2D heightfield terrain ///btHeightfieldTerrainShape simulates a 2D heightfield terrain
/** /**
The caller is responsible for maintaining the heightfield array; this The caller is responsible for maintaining the heightfield array; this
class does not make a copy. class does not make a copy.
The heightfield can be dynamic so long as the min/max height values The heightfield can be dynamic so long as the min/max height values
capture the extremes (heights must always be in that range). capture the extremes (heights must always be in that range).
The local origin of the heightfield is assumed to be the exact The local origin of the heightfield is assumed to be the exact
center (as determined by width and length and height, with each center (as determined by width and length and height, with each
axis multiplied by the localScaling). axis multiplied by the localScaling).
\b NOTE: be careful with coordinates. If you have a heightfield with a local \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 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 at the origin (0,0) and expect the heights in world coordinates to be
-100 to +500 meters. -100 to +500 meters.
Actually, the heights will be -300 to +300m, because bullet will re-center 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 the heightfield based on its AABB (which is determined by the min/max
heights). So keep in mind that once you create a btHeightfieldTerrainShape 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 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 is different to the behavior of many rendering engines, but is useful for
physics engines. physics engines.
Most (but not all) rendering and heightfield libraries assume upAxis = 1 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 (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 to be "up". Make sure your choice of axis is consistent with your rendering
system. system.
The heightfield heights are determined from the data type used for the The heightfield heights are determined from the data type used for the
heightfieldData array. heightfieldData array.
- PHY_UCHAR: height at a point is the uchar value at the - PHY_UCHAR: height at a point is the uchar value at the
grid point, multipled by heightScale. uchar isn't recommended grid point, multipled by heightScale. uchar isn't recommended
because of its inability to deal with negative values, and because of its inability to deal with negative values, and
low resolution (8-bit). low resolution (8-bit).
- PHY_SHORT: height at a point is the short int value at that grid - PHY_SHORT: height at a point is the short int value at that grid
point, multipled by heightScale. point, multipled by heightScale.
- PHY_FLOAT: height at a point is the float value at that grid - PHY_FLOAT: height at a point is the float value at that grid
point. heightScale is ignored when using the float heightfield point. heightScale is ignored when using the float heightfield
data type. data type.
Whatever the caller specifies as minHeight and maxHeight will be honored. Whatever the caller specifies as minHeight and maxHeight will be honored.
The class will not inspect the heightfield to discover the actual minimum The class will not inspect the heightfield to discover the actual minimum
or maximum heights. These values are used to determine the heightfield's or maximum heights. These values are used to determine the heightfield's
axis-aligned bounding box, multiplied by localScaling. axis-aligned bounding box, multiplied by localScaling.
For usage and testing see the TerrainDemo. For usage and testing see the TerrainDemo.
*/ */
class btHeightfieldTerrainShape : public btConcaveShape class btHeightfieldTerrainShape : public btConcaveShape
{ {
protected: protected:
btVector3 m_localAabbMin; btVector3 m_localAabbMin;
btVector3 m_localAabbMax; btVector3 m_localAabbMax;
btVector3 m_localOrigin; btVector3 m_localOrigin;
///terrain data ///terrain data
int m_heightStickWidth; int m_heightStickWidth;
int m_heightStickLength; int m_heightStickLength;
btScalar m_minHeight; btScalar m_minHeight;
btScalar m_maxHeight; btScalar m_maxHeight;
btScalar m_width; btScalar m_width;
btScalar m_length; btScalar m_length;
btScalar m_heightScale; btScalar m_heightScale;
union union
{ {
unsigned char* m_heightfieldDataUnsignedChar; unsigned char* m_heightfieldDataUnsignedChar;
short* m_heightfieldDataShort; short* m_heightfieldDataShort;
btScalar* m_heightfieldDataFloat; btScalar* m_heightfieldDataFloat;
void* m_heightfieldDataUnknown; void* m_heightfieldDataUnknown;
}; };
PHY_ScalarType m_heightDataType; PHY_ScalarType m_heightDataType;
bool m_flipQuadEdges; bool m_flipQuadEdges;
bool m_useDiamondSubdivision; bool m_useDiamondSubdivision;
int m_upAxis; int m_upAxis;
btVector3 m_localScaling; btVector3 m_localScaling;
virtual btScalar getRawHeightFieldValue(int x,int y) const; virtual btScalar getRawHeightFieldValue(int x,int y) const;
void quantizeWithClamp(int* out, const btVector3& point,int isMax) const; void quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
void getVertex(int x,int y,btVector3& vertex) const; void getVertex(int x,int y,btVector3& vertex) const;
/// protected initialization /// protected initialization
/** /**
Handles the work of constructors so that public constructors can be Handles the work of constructors so that public constructors can be
backwards-compatible without a lot of copy/paste. backwards-compatible without a lot of copy/paste.
*/ */
void initialize(int heightStickWidth, int heightStickLength, void initialize(int heightStickWidth, int heightStickLength,
void* heightfieldData, btScalar heightScale, void* heightfieldData, btScalar heightScale,
btScalar minHeight, btScalar maxHeight, int upAxis, btScalar minHeight, btScalar maxHeight, int upAxis,
PHY_ScalarType heightDataType, bool flipQuadEdges); PHY_ScalarType heightDataType, bool flipQuadEdges);
public: public:
/// preferred constructor /// preferred constructor
/** /**
This constructor supports a range of heightfield This constructor supports a range of heightfield
data types, and allows for a non-zero minimum height value. data types, and allows for a non-zero minimum height value.
heightScale is needed for any integer-based heightfield data types. heightScale is needed for any integer-based heightfield data types.
*/ */
btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,
void* heightfieldData, btScalar heightScale, void* heightfieldData, btScalar heightScale,
btScalar minHeight, btScalar maxHeight, btScalar minHeight, btScalar maxHeight,
int upAxis, PHY_ScalarType heightDataType, int upAxis, PHY_ScalarType heightDataType,
bool flipQuadEdges); bool flipQuadEdges);
/// legacy constructor /// legacy constructor
/** /**
The legacy constructor assumes the heightfield has a minimum height The legacy constructor assumes the heightfield has a minimum height
of zero. Only unsigned char or floats are supported. For legacy of zero. Only unsigned char or floats are supported. For legacy
compatibility reasons, heightScale is calculated as maxHeight / 65535 compatibility reasons, heightScale is calculated as maxHeight / 65535
(and is only used when useFloatData = false). (and is only used when useFloatData = false).
*/ */
btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges);
virtual ~btHeightfieldTerrainShape(); virtual ~btHeightfieldTerrainShape();
void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;}
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; 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 processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void setLocalScaling(const btVector3& scaling); virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const; virtual const btVector3& getLocalScaling() const;
//debugging //debugging
virtual const char* getName()const {return "HEIGHTFIELD";} virtual const char* getName()const {return "HEIGHTFIELD";}
}; };
#endif //HEIGHTFIELD_TERRAIN_SHAPE_H #endif //HEIGHTFIELD_TERRAIN_SHAPE_H

66
src/BulletCollision/CollisionShapes/btMaterial.h
View File

@@ -1,34 +1,34 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
/// This file was created by Alex Silverman /// This file was created by Alex Silverman
#ifndef MATERIAL_H #ifndef MATERIAL_H
#define MATERIAL_H #define MATERIAL_H
// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties // Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
class btMaterial class btMaterial
{ {
// public members so that materials can change due to world events // public members so that materials can change due to world events
public: public:
btScalar m_friction; btScalar m_friction;
btScalar m_restitution; btScalar m_restitution;
int pad[2]; int pad[2];
btMaterial(){} btMaterial(){}
btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
}; };
#endif // MATERIAL_H #endif // MATERIAL_H

90
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp
View File

@@ -1,45 +1,45 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
/// This file was created by Alex Silverman /// This file was created by Alex Silverman
#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h" #include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" //#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
///Obtains the material for a specific triangle ///Obtains the material for a specific triangle
const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex) const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
{ {
const unsigned char * materialBase = 0; const unsigned char * materialBase = 0;
int numMaterials; int numMaterials;
PHY_ScalarType materialType; PHY_ScalarType materialType;
int materialStride; int materialStride;
const unsigned char * triangleMaterialBase = 0; const unsigned char * triangleMaterialBase = 0;
int numTriangles; int numTriangles;
int triangleMaterialStride; int triangleMaterialStride;
PHY_ScalarType triangleType; PHY_ScalarType triangleType;
((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride, ((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
&triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID); &triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
// return the pointer to the place with the friction for the triangle // return the pointer to the place with the friction for the triangle
// TODO: This depends on whether it's a moving mesh or not // TODO: This depends on whether it's a moving mesh or not
// BUG IN GIMPACT // BUG IN GIMPACT
//return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]); //return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)])); int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride])); btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
return (matVal); return (matVal);
} }

246
src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
View File

@@ -1,123 +1,123 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
/// This file was created by Alex Silverman /// This file was created by Alex Silverman
#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H #ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H #define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#include "btBvhTriangleMeshShape.h" #include "btBvhTriangleMeshShape.h"
#include "btMaterial.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. ///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 ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
{ {
btAlignedObjectArray <btMaterial*> m_materialList; btAlignedObjectArray <btMaterial*> m_materialList;
int ** m_triangleMaterials; int ** m_triangleMaterials;
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;} btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true): btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh) btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
{ {
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3]; btVector3 m_triangle[3];
const unsigned char *vertexbase; const unsigned char *vertexbase;
int numverts; int numverts;
PHY_ScalarType type; PHY_ScalarType type;
int stride; int stride;
const unsigned char *indexbase; const unsigned char *indexbase;
int indexstride; int indexstride;
int numfaces; int numfaces;
PHY_ScalarType indicestype; PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++) for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{ {
m_meshInterface->getLockedReadOnlyVertexIndexBase( m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase, &vertexbase,
numverts, numverts,
type, type,
stride, stride,
&indexbase, &indexbase,
indexstride, indexstride,
numfaces, numfaces,
indicestype, indicestype,
i); i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16)); //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 ///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): btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh) btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
{ {
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE; m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3]; btVector3 m_triangle[3];
const unsigned char *vertexbase; const unsigned char *vertexbase;
int numverts; int numverts;
PHY_ScalarType type; PHY_ScalarType type;
int stride; int stride;
const unsigned char *indexbase; const unsigned char *indexbase;
int indexstride; int indexstride;
int numfaces; int numfaces;
PHY_ScalarType indicestype; PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16)); //m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++) for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{ {
m_meshInterface->getLockedReadOnlyVertexIndexBase( m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase, &vertexbase,
numverts, numverts,
type, type,
stride, stride,
&indexbase, &indexbase,
indexstride, indexstride,
numfaces, numfaces,
indicestype, indicestype,
i); i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16)); //m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
} }
} }
virtual ~btMultimaterialTriangleMeshShape() virtual ~btMultimaterialTriangleMeshShape()
{ {
/* /*
for(int i = 0; i < m_meshInterface->getNumSubParts(); i++) for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
{ {
btAlignedFree(m_materialValues[i]); btAlignedFree(m_materialValues[i]);
m_materialLookup[i] = NULL; m_materialLookup[i] = NULL;
} }
btAlignedFree(m_materialValues); btAlignedFree(m_materialValues);
m_materialLookup = NULL; m_materialLookup = NULL;
*/ */
} }
//debugging //debugging
virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";} virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";}
///Obtains the material for a specific triangle ///Obtains the material for a specific triangle
const btMaterial * getMaterialProperties(int partID, int triIndex); const btMaterial * getMaterialProperties(int partID, int triIndex);
} }
; ;
#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H #endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H

240
src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp
View File

@@ -1,120 +1,120 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btScaledBvhTriangleMeshShape.h" #include "btScaledBvhTriangleMeshShape.h"
btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling) btScaledBvhTriangleMeshShape::btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling)
:m_localScaling(localScaling),m_bvhTriMeshShape(childShape) :m_localScaling(localScaling),m_bvhTriMeshShape(childShape)
{ {
m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE; m_shapeType = SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE;
} }
btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape() btScaledBvhTriangleMeshShape::~btScaledBvhTriangleMeshShape()
{ {
} }
class btScaledTriangleCallback : public btTriangleCallback class btScaledTriangleCallback : public btTriangleCallback
{ {
btTriangleCallback* m_originalCallback; btTriangleCallback* m_originalCallback;
btVector3 m_localScaling; btVector3 m_localScaling;
public: public:
btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling) btScaledTriangleCallback(btTriangleCallback* originalCallback,const btVector3& localScaling)
:m_originalCallback(originalCallback), :m_originalCallback(originalCallback),
m_localScaling(localScaling) m_localScaling(localScaling)
{ {
} }
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{ {
btVector3 newTriangle[3]; btVector3 newTriangle[3];
newTriangle[0] = triangle[0]*m_localScaling; newTriangle[0] = triangle[0]*m_localScaling;
newTriangle[1] = triangle[1]*m_localScaling; newTriangle[1] = triangle[1]*m_localScaling;
newTriangle[2] = triangle[2]*m_localScaling; newTriangle[2] = triangle[2]*m_localScaling;
m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex); m_originalCallback->processTriangle(&newTriangle[0],partId,triangleIndex);
} }
}; };
void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const void btScaledBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
{ {
btScaledTriangleCallback scaledCallback(callback,m_localScaling); btScaledTriangleCallback scaledCallback(callback,m_localScaling);
btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ()); btVector3 invLocalScaling(1.f/m_localScaling.getX(),1.f/m_localScaling.getY(),1.f/m_localScaling.getZ());
btVector3 scaledAabbMin,scaledAabbMax; btVector3 scaledAabbMin,scaledAabbMax;
///support negative scaling ///support negative scaling
scaledAabbMin[0] = m_localScaling.getX() >= 0. ? aabbMin[0] * invLocalScaling[0] : aabbMax[0] * invLocalScaling[0]; 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[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]; 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[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[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]; scaledAabbMax[2] = m_localScaling.getZ() <= 0. ? aabbMin[2] * invLocalScaling[2] : aabbMax[2] * invLocalScaling[2];
m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax); m_bvhTriMeshShape->processAllTriangles(&scaledCallback,scaledAabbMin,scaledAabbMax);
} }
void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const void btScaledBvhTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
{ {
btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin(); btVector3 localAabbMin = m_bvhTriMeshShape->getLocalAabbMin();
btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax(); btVector3 localAabbMax = m_bvhTriMeshShape->getLocalAabbMax();
btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling; btVector3 tmpLocalAabbMin = localAabbMin * m_localScaling;
btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling; btVector3 tmpLocalAabbMax = localAabbMax * m_localScaling;
localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; localAabbMin[0] = (m_localScaling.getX() >= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; localAabbMin[1] = (m_localScaling.getY() >= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; localAabbMin[2] = (m_localScaling.getZ() >= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0]; localAabbMax[0] = (m_localScaling.getX() <= 0.) ? tmpLocalAabbMin[0] : tmpLocalAabbMax[0];
localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1]; localAabbMax[1] = (m_localScaling.getY() <= 0.) ? tmpLocalAabbMin[1] : tmpLocalAabbMax[1];
localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2]; localAabbMax[2] = (m_localScaling.getZ() <= 0.) ? tmpLocalAabbMin[2] : tmpLocalAabbMax[2];
btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin); btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
btScalar margin = m_bvhTriMeshShape->getMargin(); btScalar margin = m_bvhTriMeshShape->getMargin();
localHalfExtents += btVector3(margin,margin,margin); localHalfExtents += btVector3(margin,margin,margin);
btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin); btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute(); btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter); btVector3 center = trans(localCenter);
btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
abs_b[1].dot(localHalfExtents), abs_b[1].dot(localHalfExtents),
abs_b[2].dot(localHalfExtents)); abs_b[2].dot(localHalfExtents));
aabbMin = center - extent; aabbMin = center - extent;
aabbMax = center + extent; aabbMax = center + extent;
} }
void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) void btScaledBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling)
{ {
m_localScaling = scaling; m_localScaling = scaling;
} }
const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const const btVector3& btScaledBvhTriangleMeshShape::getLocalScaling() const
{ {
return m_localScaling; return m_localScaling;
} }
void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const void btScaledBvhTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{ {
///don't make this a movable object! ///don't make this a movable object!
// btAssert(0); // btAssert(0);
} }

124
src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h
View File

@@ -1,62 +1,62 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H #ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H #define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape. ///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' ///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
{ {
btVector3 m_localScaling; btVector3 m_localScaling;
btBvhTriangleMeshShape* m_bvhTriMeshShape; btBvhTriangleMeshShape* m_bvhTriMeshShape;
public: public:
btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling); btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
virtual ~btScaledBvhTriangleMeshShape(); virtual ~btScaledBvhTriangleMeshShape();
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void setLocalScaling(const btVector3& scaling); virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const; virtual const btVector3& getLocalScaling() const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
btBvhTriangleMeshShape* getChildShape() btBvhTriangleMeshShape* getChildShape()
{ {
return m_bvhTriMeshShape; return m_bvhTriMeshShape;
} }
const btBvhTriangleMeshShape* getChildShape() const const btBvhTriangleMeshShape* getChildShape() const
{ {
return m_bvhTriMeshShape; return m_bvhTriMeshShape;
} }
//debugging //debugging
virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";} virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";}
}; };
#endif //BVH_TRIANGLE_MESH_SHAPE_H #endif //BVH_TRIANGLE_MESH_SHAPE_H

328
src/BulletCollision/CollisionShapes/btShapeHull.cpp
View File

@@ -1,164 +1,164 @@
/* /*
btbtShapeHull implemented by John McCutchan. btbtShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btShapeHull.h" #include "btShapeHull.h"
#include "LinearMath/btConvexHull.h" #include "LinearMath/btConvexHull.h"
#define NUM_UNITSPHERE_POINTS 42 #define NUM_UNITSPHERE_POINTS 42
static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
{ {
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
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.276388) , btScalar(-0.850649),btScalar(-0.447219)),
btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)), btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)), 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.447220)), 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.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.276388) , btScalar(0.850649),btScalar(0.447219)),
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)), btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)), btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
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)), btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)), btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)), btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)), btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)), btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)), btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)), btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)), btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)), 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.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.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(0.000000) , btScalar(-1.000000),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.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.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.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(-0.000000) , btScalar(1.000000),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.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)), btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)), btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)), btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)), btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)), btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)), btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)), 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.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)) btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
}; };
btShapeHull::btShapeHull (const btConvexShape* shape) btShapeHull::btShapeHull (const btConvexShape* shape)
{ {
m_shape = shape; m_shape = shape;
m_vertices.clear (); m_vertices.clear ();
m_indices.clear(); m_indices.clear();
m_numIndices = 0; m_numIndices = 0;
} }
btShapeHull::~btShapeHull () btShapeHull::~btShapeHull ()
{ {
m_indices.clear(); m_indices.clear();
m_vertices.clear (); m_vertices.clear ();
} }
bool bool
btShapeHull::buildHull (btScalar /*margin*/) btShapeHull::buildHull (btScalar /*margin*/)
{ {
int numSampleDirections = NUM_UNITSPHERE_POINTS; int numSampleDirections = NUM_UNITSPHERE_POINTS;
{ {
int numPDA = m_shape->getNumPreferredPenetrationDirections(); int numPDA = m_shape->getNumPreferredPenetrationDirections();
if (numPDA) if (numPDA)
{ {
for (int i=0;i<numPDA;i++) for (int i=0;i<numPDA;i++)
{ {
btVector3 norm; btVector3 norm;
m_shape->getPreferredPenetrationDirection(i,norm); m_shape->getPreferredPenetrationDirection(i,norm);
btUnitSpherePoints[numSampleDirections] = norm; btUnitSpherePoints[numSampleDirections] = norm;
numSampleDirections++; numSampleDirections++;
} }
} }
} }
btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i; int i;
for (i = 0; i < numSampleDirections; i++) for (i = 0; i < numSampleDirections; i++)
{ {
supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]); supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
} }
HullDesc hd; HullDesc hd;
hd.mFlags = QF_TRIANGLES; hd.mFlags = QF_TRIANGLES;
hd.mVcount = static_cast<unsigned int>(numSampleDirections); hd.mVcount = static_cast<unsigned int>(numSampleDirections);
#ifdef BT_USE_DOUBLE_PRECISION #ifdef BT_USE_DOUBLE_PRECISION
hd.mVertices = &supportPoints[0]; hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof(btVector3); hd.mVertexStride = sizeof(btVector3);
#else #else
hd.mVertices = &supportPoints[0]; hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof (btVector3); hd.mVertexStride = sizeof (btVector3);
#endif #endif
HullLibrary hl; HullLibrary hl;
HullResult hr; HullResult hr;
if (hl.CreateConvexHull (hd, hr) == QE_FAIL) if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
{ {
return false; return false;
} }
m_vertices.resize (static_cast<int>(hr.mNumOutputVertices)); m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++) for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
{ {
m_vertices[i] = hr.m_OutputVertices[i]; m_vertices[i] = hr.m_OutputVertices[i];
} }
m_numIndices = hr.mNumIndices; m_numIndices = hr.mNumIndices;
m_indices.resize(static_cast<int>(m_numIndices)); m_indices.resize(static_cast<int>(m_numIndices));
for (i = 0; i < static_cast<int>(m_numIndices); i++) for (i = 0; i < static_cast<int>(m_numIndices); i++)
{ {
m_indices[i] = hr.m_Indices[i]; m_indices[i] = hr.m_Indices[i];
} }
// free temporary hull result that we just copied // free temporary hull result that we just copied
hl.ReleaseResult (hr); hl.ReleaseResult (hr);
return true; return true;
} }
int int
btShapeHull::numTriangles () const btShapeHull::numTriangles () const
{ {
return static_cast<int>(m_numIndices / 3); return static_cast<int>(m_numIndices / 3);
} }
int int
btShapeHull::numVertices () const btShapeHull::numVertices () const
{ {
return m_vertices.size (); return m_vertices.size ();
} }
int int
btShapeHull::numIndices () const btShapeHull::numIndices () const
{ {
return static_cast<int>(m_numIndices); return static_cast<int>(m_numIndices);
} }

112
src/BulletCollision/CollisionShapes/btShapeHull.h
View File

@@ -1,56 +1,56 @@
/* /*
btShapeHull implemented by John McCutchan. btShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef _SHAPE_HULL_H #ifndef _SHAPE_HULL_H
#define _SHAPE_HULL_H #define _SHAPE_HULL_H
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "BulletCollision/CollisionShapes/btConvexShape.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. ///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 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. ///It approximates the convex hull using the supporting vertex of 42 directions.
class btShapeHull class btShapeHull
{ {
public: public:
btShapeHull (const btConvexShape* shape); btShapeHull (const btConvexShape* shape);
~btShapeHull (); ~btShapeHull ();
bool buildHull (btScalar margin); bool buildHull (btScalar margin);
int numTriangles () const; int numTriangles () const;
int numVertices () const; int numVertices () const;
int numIndices () const; int numIndices () const;
const btVector3* getVertexPointer() const const btVector3* getVertexPointer() const
{ {
return &m_vertices[0]; return &m_vertices[0];
} }
const unsigned int* getIndexPointer() const const unsigned int* getIndexPointer() const
{ {
return &m_indices[0]; return &m_indices[0];
} }
protected: protected:
btAlignedObjectArray<btVector3> m_vertices; btAlignedObjectArray<btVector3> m_vertices;
btAlignedObjectArray<unsigned int> m_indices; btAlignedObjectArray<unsigned int> m_indices;
unsigned int m_numIndices; unsigned int m_numIndices;
const btConvexShape* m_shape; const btConvexShape* m_shape;
}; };
#endif //_SHAPE_HULL_H #endif //_SHAPE_HULL_H

70
src/BulletCollision/CollisionShapes/btTriangleBuffer.cpp
View File

@@ -1,35 +1,35 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btTriangleBuffer.h" #include "btTriangleBuffer.h"
void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex) void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex)
{ {
btTriangle tri; btTriangle tri;
tri.m_vertex0 = triangle[0]; tri.m_vertex0 = triangle[0];
tri.m_vertex1 = triangle[1]; tri.m_vertex1 = triangle[1];
tri.m_vertex2 = triangle[2]; tri.m_vertex2 = triangle[2];
tri.m_partId = partId; tri.m_partId = partId;
tri.m_triangleIndex = triangleIndex; tri.m_triangleIndex = triangleIndex;
m_triangleBuffer.push_back(tri); m_triangleBuffer.push_back(tri);
} }

138
src/BulletCollision/CollisionShapes/btTriangleBuffer.h
View File

@@ -1,69 +1,69 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_TRIANGLE_BUFFER_H #ifndef BT_TRIANGLE_BUFFER_H
#define BT_TRIANGLE_BUFFER_H #define BT_TRIANGLE_BUFFER_H
#include "btTriangleCallback.h" #include "btTriangleCallback.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
struct btTriangle struct btTriangle
{ {
btVector3 m_vertex0; btVector3 m_vertex0;
btVector3 m_vertex1; btVector3 m_vertex1;
btVector3 m_vertex2; btVector3 m_vertex2;
int m_partId; int m_partId;
int m_triangleIndex; int m_triangleIndex;
}; };
///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles' ///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: ///Example usage of this class:
/// btTriangleBuffer triBuf; /// btTriangleBuffer triBuf;
/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax); /// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
/// for (int i=0;i<triBuf.getNumTriangles();i++) /// for (int i=0;i<triBuf.getNumTriangles();i++)
/// { /// {
/// const btTriangle& tri = triBuf.getTriangle(i); /// const btTriangle& tri = triBuf.getTriangle(i);
/// //do something useful here with the triangle /// //do something useful here with the triangle
/// } /// }
class btTriangleBuffer : public btTriangleCallback class btTriangleBuffer : public btTriangleCallback
{ {
btAlignedObjectArray<btTriangle> m_triangleBuffer; btAlignedObjectArray<btTriangle> m_triangleBuffer;
public: public:
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
int getNumTriangles() const int getNumTriangles() const
{ {
return int(m_triangleBuffer.size()); return int(m_triangleBuffer.size());
} }
const btTriangle& getTriangle(int index) const const btTriangle& getTriangle(int index) const
{ {
return m_triangleBuffer[index]; return m_triangleBuffer[index];
} }
void clearBuffer() void clearBuffer()
{ {
m_triangleBuffer.clear(); m_triangleBuffer.clear();
} }
}; };
#endif //BT_TRIANGLE_BUFFER_H #endif //BT_TRIANGLE_BUFFER_H

1
src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
View File

@@ -82,6 +82,7 @@ bool btTriangleIndexVertexArray::hasPremadeAabb() const
return (m_hasAabb == 1); return (m_hasAabb == 1);
} }
void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
{ {
m_aabbMin = aabbMin; m_aabbMin = aabbMin;

173
src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
View File

@@ -1,86 +1,87 @@
/*
Bullet Continuous Collision Detection and Physics Library /*
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 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. This software is provided 'as-is', without any express or implied warranty.
Permission is granted to anyone to use this software for any purpose, In no event will the authors be held liable for any damages arising from the use of this software.
including commercial applications, and to alter it and redistribute it freely, Permission is granted to anyone to use this software for any purpose,
subject to the following restrictions: 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. 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.
3. This notice may not be removed or altered from any source distribution. 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
///This file was created by Alex Silverman
#include "btTriangleIndexVertexMaterialArray.h"
#include "btTriangleIndexVertexMaterialArray.h"
btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
int numVertices,btScalar* vertexBase,int vertexStride, btTriangleIndexVertexMaterialArray::btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
int numMaterials, unsigned char* materialBase, int materialStride, int numVertices,btScalar* vertexBase,int vertexStride,
int* triangleMaterialsBase, int materialIndexStride) : int numMaterials, unsigned char* materialBase, int materialStride,
btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride) int* triangleMaterialsBase, int materialIndexStride) :
{ btTriangleIndexVertexArray(numTriangles, triangleIndexBase, triangleIndexStride, numVertices, vertexBase, vertexStride)
btMaterialProperties mat; {
btMaterialProperties mat;
mat.m_numMaterials = numMaterials;
mat.m_materialBase = materialBase; mat.m_numMaterials = numMaterials;
mat.m_materialStride = materialStride; mat.m_materialBase = materialBase;
#ifdef BT_USE_DOUBLE_PRECISION mat.m_materialStride = materialStride;
mat.m_materialType = PHY_DOUBLE; #ifdef BT_USE_DOUBLE_PRECISION
#else mat.m_materialType = PHY_DOUBLE;
mat.m_materialType = PHY_FLOAT; #else
#endif mat.m_materialType = PHY_FLOAT;
#endif
mat.m_numTriangles = numTriangles;
mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase; mat.m_numTriangles = numTriangles;
mat.m_triangleMaterialStride = materialIndexStride; mat.m_triangleMaterialsBase = (unsigned char *)triangleMaterialsBase;
mat.m_triangleType = PHY_INTEGER; mat.m_triangleMaterialStride = materialIndexStride;
mat.m_triangleType = PHY_INTEGER;
addMaterialProperties(mat);
} 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) 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() ); {
btAssert(subpart< getNumSubParts() );
btMaterialProperties& mats = m_materials[subpart];
btMaterialProperties& mats = m_materials[subpart];
numMaterials = mats.m_numMaterials;
(*materialBase) = (unsigned char *) mats.m_materialBase; numMaterials = mats.m_numMaterials;
#ifdef BT_USE_DOUBLE_PRECISION (*materialBase) = (unsigned char *) mats.m_materialBase;
materialType = PHY_DOUBLE; #ifdef BT_USE_DOUBLE_PRECISION
#else materialType = PHY_DOUBLE;
materialType = PHY_FLOAT; #else
#endif materialType = PHY_FLOAT;
materialStride = mats.m_materialStride; #endif
materialStride = mats.m_materialStride;
numTriangles = mats.m_numTriangles;
(*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase; numTriangles = mats.m_numTriangles;
triangleMaterialStride = mats.m_triangleMaterialStride; (*triangleMaterialBase) = (unsigned char *)mats.m_triangleMaterialsBase;
triangleType = mats.m_triangleType; 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) 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]; {
btMaterialProperties& mats = m_materials[subpart];
numMaterials = mats.m_numMaterials;
(*materialBase) = (const unsigned char *) mats.m_materialBase; numMaterials = mats.m_numMaterials;
#ifdef BT_USE_DOUBLE_PRECISION (*materialBase) = (const unsigned char *) mats.m_materialBase;
materialType = PHY_DOUBLE; #ifdef BT_USE_DOUBLE_PRECISION
#else materialType = PHY_DOUBLE;
materialType = PHY_FLOAT; #else
#endif materialType = PHY_FLOAT;
materialStride = mats.m_materialStride; #endif
materialStride = mats.m_materialStride;
numTriangles = mats.m_numTriangles;
(*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase; numTriangles = mats.m_numTriangles;
triangleMaterialStride = mats.m_triangleMaterialStride; (*triangleMaterialBase) = (const unsigned char *)mats.m_triangleMaterialsBase;
triangleType = mats.m_triangleType; triangleMaterialStride = mats.m_triangleMaterialStride;
} triangleType = mats.m_triangleType;
}

168
src/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h
View File

@@ -1,84 +1,84 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
///This file was created by Alex Silverman ///This file was created by Alex Silverman
#ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H #ifndef BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
#define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H #define BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H
#include "btTriangleIndexVertexArray.h" #include "btTriangleIndexVertexArray.h"
ATTRIBUTE_ALIGNED16( struct) btMaterialProperties ATTRIBUTE_ALIGNED16( struct) btMaterialProperties
{ {
///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution ///m_materialBase ==========> 2 btScalar values make up one material, friction then restitution
int m_numMaterials; int m_numMaterials;
const unsigned char * m_materialBase; const unsigned char * m_materialBase;
int m_materialStride; int m_materialStride;
PHY_ScalarType m_materialType; PHY_ScalarType m_materialType;
///m_numTriangles <=========== This exists in the btIndexedMesh object for the same subpart, but since we're ///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 /// padding the structure, it can be reproduced at no real cost
///m_triangleMaterials =====> 1 integer value makes up one entry ///m_triangleMaterials =====> 1 integer value makes up one entry
/// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5 /// eg: m_triangleMaterials[1] = 5; // This will set triangle 2 to use material 5
int m_numTriangles; int m_numTriangles;
const unsigned char * m_triangleMaterialsBase; const unsigned char * m_triangleMaterialsBase;
int m_triangleMaterialStride; int m_triangleMaterialStride;
///m_triangleType <========== Automatically set in addMaterialProperties ///m_triangleType <========== Automatically set in addMaterialProperties
PHY_ScalarType m_triangleType; PHY_ScalarType m_triangleType;
}; };
typedef btAlignedObjectArray<btMaterialProperties> MaterialArray; typedef btAlignedObjectArray<btMaterialProperties> MaterialArray;
///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray ///Teh btTriangleIndexVertexMaterialArray is built on TriangleIndexVertexArray
///The addition of a material array allows for the utilization of the partID and ///The addition of a material array allows for the utilization of the partID and
///triangleIndex that are returned in the ContactAddedCallback. As with ///triangleIndex that are returned in the ContactAddedCallback. As with
///TriangleIndexVertexArray, no duplicate is made of the material data, so it ///TriangleIndexVertexArray, no duplicate is made of the material data, so it
///is the users responsibility to maintain the array during the lifetime of the ///is the users responsibility to maintain the array during the lifetime of the
///TriangleIndexVertexMaterialArray. ///TriangleIndexVertexMaterialArray.
ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray ATTRIBUTE_ALIGNED16(class) btTriangleIndexVertexMaterialArray : public btTriangleIndexVertexArray
{ {
protected: protected:
MaterialArray m_materials; MaterialArray m_materials;
public: public:
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btTriangleIndexVertexMaterialArray() btTriangleIndexVertexMaterialArray()
{ {
} }
btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride, btTriangleIndexVertexMaterialArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,
int numVertices,btScalar* vertexBase,int vertexStride, int numVertices,btScalar* vertexBase,int vertexStride,
int numMaterials, unsigned char* materialBase, int materialStride, int numMaterials, unsigned char* materialBase, int materialStride,
int* triangleMaterialsBase, int materialIndexStride); int* triangleMaterialsBase, int materialIndexStride);
virtual ~btTriangleIndexVertexMaterialArray() {} virtual ~btTriangleIndexVertexMaterialArray() {}
void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER) void addMaterialProperties(const btMaterialProperties& mat, PHY_ScalarType triangleType = PHY_INTEGER)
{ {
m_materials.push_back(mat); m_materials.push_back(mat);
m_materials[m_materials.size()-1].m_triangleType = triangleType; m_materials[m_materials.size()-1].m_triangleType = triangleType;
} }
virtual void getLockedMaterialBase(unsigned char **materialBase, int& numMaterials, PHY_ScalarType& materialType, int& materialStride, 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); 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, 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); const unsigned char ** triangleMaterialBase, int& numTriangles, int& triangleMaterialStride, PHY_ScalarType& triangleType, int subpart = 0);
} }
; ;
#endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H #endif //BT_MULTIMATERIAL_TRIANGLE_INDEX_VERTEX_ARRAY_H

1886
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
View File

File diff suppressed because it is too large Load Diff

142
src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h
View File

@@ -1,71 +1,71 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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 In no event will the authors be held liable for any damages arising from the
use of this software. use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it including commercial applications, and to alter it and redistribute it
freely, freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not 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 claim that you wrote the original software. If you use this software in a
product, an acknowledgment in the product documentation would be appreciated product, an acknowledgment in the product documentation would be appreciated
but is not required. but is not required.
2. Altered source versions must be plainly marked as such, and must not be 2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software. misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
/* /*
GJK-EPA collision solver by Nathanael Presson, 2008 GJK-EPA collision solver by Nathanael Presson, 2008
*/ */
#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ #ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ #define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
///btGjkEpaSolver contributed under zlib by Nathanael Presson ///btGjkEpaSolver contributed under zlib by Nathanael Presson
struct btGjkEpaSolver2 struct btGjkEpaSolver2
{ {
struct sResults struct sResults
{ {
enum eStatus enum eStatus
{ {
Separated, /* Shapes doesnt penetrate */ Separated, /* Shapes doesnt penetrate */
Penetrating, /* Shapes are penetrating */ Penetrating, /* Shapes are penetrating */
GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ 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 */ EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
} status; } status;
btVector3 witnesses[2]; btVector3 witnesses[2];
btVector3 normal; btVector3 normal;
btScalar distance; btScalar distance;
}; };
static int StackSizeRequirement(); static int StackSizeRequirement();
static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0, static bool Distance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1, const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess, const btVector3& guess,
sResults& results); sResults& results);
static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0, static bool Penetration(const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1, const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess, const btVector3& guess,
sResults& results, sResults& results,
bool usemargins=true); bool usemargins=true);
static btScalar SignedDistance( const btVector3& position, static btScalar SignedDistance( const btVector3& position,
btScalar margin, btScalar margin,
const btConvexShape* shape, const btConvexShape* shape,
const btTransform& wtrs, const btTransform& wtrs,
sResults& results); sResults& results);
static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0, static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs0,
const btConvexShape* shape1,const btTransform& wtrs1, const btConvexShape* shape1,const btTransform& wtrs1,
const btVector3& guess, const btVector3& guess,
sResults& results); sResults& results);
}; };
#endif #endif

108
src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
View File

@@ -1,54 +1,54 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
EPA Copyright (c) Ricardo Padrela 2006 EPA Copyright (c) Ricardo Padrela 2006
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "btGjkEpaPenetrationDepthSolver.h" #include "btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB, const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB, const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc ) class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
{ {
(void)debugDraw; (void)debugDraw;
(void)v; (void)v;
(void)simplexSolver; (void)simplexSolver;
const btScalar radialmargin(btScalar(0.)); const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin()); btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin());
btGjkEpaSolver2::sResults results; btGjkEpaSolver2::sResults results;
if(btGjkEpaSolver2::Penetration(pConvexA,transformA, if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB, pConvexB,transformB,
guessVector,results)) guessVector,results))
{ {
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0]; wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1]; wWitnessOnB = results.witnesses[1];
return true; return true;
} }
return false; return false;
} }

86
src/BulletDynamics/Character/btCharacterControllerInterface.h
View File

@@ -1,43 +1,43 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef CHARACTER_CONTROLLER_INTERFACE_H #ifndef CHARACTER_CONTROLLER_INTERFACE_H
#define CHARACTER_CONTROLLER_INTERFACE_H #define CHARACTER_CONTROLLER_INTERFACE_H
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
class btCollisionShape; class btCollisionShape;
class btRigidBody; class btRigidBody;
class btCollisionWorld; class btCollisionWorld;
class btCharacterControllerInterface class btCharacterControllerInterface
{ {
public: public:
btCharacterControllerInterface () {}; btCharacterControllerInterface () {};
virtual ~btCharacterControllerInterface () {}; virtual ~btCharacterControllerInterface () {};
virtual void setWalkDirection(const btVector3& walkDirection) = 0; virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void reset () = 0; virtual void reset () = 0;
virtual void warp (const btVector3& origin) = 0; virtual void warp (const btVector3& origin) = 0;
virtual void preStep ( btCollisionWorld* collisionWorld) = 0; virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0; virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0; virtual bool canJump () const = 0;
virtual void jump () = 0; virtual void jump () = 0;
virtual bool onGround () const = 0; virtual bool onGround () const = 0;
}; };
#endif #endif

942
src/BulletDynamics/Character/btKinematicCharacterController.cpp
View File

@@ -1,471 +1,471 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/CollisionDispatch/btGhostObject.h" #include "BulletCollision/CollisionDispatch/btGhostObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "LinearMath/btDefaultMotionState.h" #include "LinearMath/btDefaultMotionState.h"
#include "btKinematicCharacterController.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) }; 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, ///@todo Interact with dynamic objects,
///Ride kinematicly animated platforms properly ///Ride kinematicly animated platforms properly
///More realistic (or maybe just a config option) falling ///More realistic (or maybe just a config option) falling
/// -> Should integrate falling velocity manually and use that in stepDown() /// -> Should integrate falling velocity manually and use that in stepDown()
///Support jumping ///Support jumping
///Support ducking ///Support ducking
class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback class btKinematicClosestNotMeRayResultCallback : public btCollisionWorld::ClosestRayResultCallback
{ {
public: public:
btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) btKinematicClosestNotMeRayResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestRayResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
{ {
m_me = me; m_me = me;
} }
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace) virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
{ {
if (rayResult.m_collisionObject == m_me) if (rayResult.m_collisionObject == m_me)
return 1.0; return 1.0;
return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace); return ClosestRayResultCallback::addSingleResult (rayResult, normalInWorldSpace);
} }
protected: protected:
btCollisionObject* m_me; btCollisionObject* m_me;
}; };
class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback class btKinematicClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{ {
public: public:
btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0)) btKinematicClosestNotMeConvexResultCallback (btCollisionObject* me) : btCollisionWorld::ClosestConvexResultCallback(btVector3(0.0, 0.0, 0.0), btVector3(0.0, 0.0, 0.0))
{ {
m_me = me; m_me = me;
} }
virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
{ {
if (convexResult.m_hitCollisionObject == m_me) if (convexResult.m_hitCollisionObject == m_me)
return 1.0; return 1.0;
return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
} }
protected: protected:
btCollisionObject* m_me; btCollisionObject* m_me;
}; };
/* /*
* Returns the reflection direction of a ray going 'direction' hitting a surface with normal 'normal' * 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 * from: http://www-cs-students.stanford.edu/~adityagp/final/node3.html
*/ */
btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal) btVector3 btKinematicCharacterController::computeReflectionDirection (const btVector3& direction, const btVector3& normal)
{ {
return direction - (btScalar(2.0) * direction.dot(normal)) * normal; return direction - (btScalar(2.0) * direction.dot(normal)) * normal;
} }
/* /*
* Returns the portion of 'direction' that is parallel to 'normal' * Returns the portion of 'direction' that is parallel to 'normal'
*/ */
btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal) btVector3 btKinematicCharacterController::parallelComponent (const btVector3& direction, const btVector3& normal)
{ {
btScalar magnitude = direction.dot(normal); btScalar magnitude = direction.dot(normal);
return normal * magnitude; return normal * magnitude;
} }
/* /*
* Returns the portion of 'direction' that is perpindicular to 'normal' * Returns the portion of 'direction' that is perpindicular to 'normal'
*/ */
btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal) btVector3 btKinematicCharacterController::perpindicularComponent (const btVector3& direction, const btVector3& normal)
{ {
return direction - parallelComponent(direction, normal); return direction - parallelComponent(direction, normal);
} }
btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis) btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
{ {
m_upAxis = upAxis; m_upAxis = upAxis;
m_addedMargin = 0.02f; m_addedMargin = 0.02f;
m_walkDirection.setValue(0,0,0); m_walkDirection.setValue(0,0,0);
m_useGhostObjectSweepTest = true; m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject; m_ghostObject = ghostObject;
m_stepHeight = stepHeight; m_stepHeight = stepHeight;
m_turnAngle = btScalar(0.0); m_turnAngle = btScalar(0.0);
m_convexShape=convexShape; m_convexShape=convexShape;
} }
btKinematicCharacterController::~btKinematicCharacterController () btKinematicCharacterController::~btKinematicCharacterController ()
{ {
} }
btPairCachingGhostObject* btKinematicCharacterController::getGhostObject() btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
{ {
return m_ghostObject; return m_ghostObject;
} }
bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld) bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
{ {
bool penetration = false; bool penetration = false;
collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher()); collisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_ghostObject->getOverlappingPairCache(), collisionWorld->getDispatchInfo(), collisionWorld->getDispatcher());
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
btScalar maxPen = btScalar(0.0); btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++) for (int i = 0; i < m_ghostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{ {
m_manifoldArray.resize(0); m_manifoldArray.resize(0);
btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i]; btBroadphasePair* collisionPair = &m_ghostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
if (collisionPair->m_algorithm) if (collisionPair->m_algorithm)
collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray); collisionPair->m_algorithm->getAllContactManifolds(m_manifoldArray);
for (int j=0;j<m_manifoldArray.size();j++) for (int j=0;j<m_manifoldArray.size();j++)
{ {
btPersistentManifold* manifold = m_manifoldArray[j]; btPersistentManifold* manifold = m_manifoldArray[j];
btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0); btScalar directionSign = manifold->getBody0() == m_ghostObject ? btScalar(-1.0) : btScalar(1.0);
for (int p=0;p<manifold->getNumContacts();p++) for (int p=0;p<manifold->getNumContacts();p++)
{ {
const btManifoldPoint&pt = manifold->getContactPoint(p); const btManifoldPoint&pt = manifold->getContactPoint(p);
if (pt.getDistance() < 0.0) if (pt.getDistance() < 0.0)
{ {
if (pt.getDistance() < maxPen) if (pt.getDistance() < maxPen)
{ {
maxPen = pt.getDistance(); maxPen = pt.getDistance();
m_touchingNormal = pt.m_normalWorldOnB * directionSign;//?? m_touchingNormal = pt.m_normalWorldOnB * directionSign;//??
} }
m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2); m_currentPosition += pt.m_normalWorldOnB * directionSign * pt.getDistance() * btScalar(0.2);
penetration = true; penetration = true;
} else { } else {
//printf("touching %f\n", pt.getDistance()); //printf("touching %f\n", pt.getDistance());
} }
} }
//manifold->clearManifold(); //manifold->clearManifold();
} }
} }
btTransform newTrans = m_ghostObject->getWorldTransform(); btTransform newTrans = m_ghostObject->getWorldTransform();
newTrans.setOrigin(m_currentPosition); newTrans.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(newTrans); m_ghostObject->setWorldTransform(newTrans);
// printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
return penetration; return penetration;
} }
void btKinematicCharacterController::stepUp ( btCollisionWorld* world) void btKinematicCharacterController::stepUp ( btCollisionWorld* world)
{ {
// phase 1: up // phase 1: up
btTransform start, end; btTransform start, end;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight; m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
start.setIdentity (); start.setIdentity ();
end.setIdentity (); end.setIdentity ();
/* FIXME: Handle penetration properly */ /* FIXME: Handle penetration properly */
start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f)); start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
end.setOrigin (m_targetPosition); end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
if (m_useGhostObjectSweepTest) if (m_useGhostObjectSweepTest)
{ {
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration); m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, world->getDispatchInfo().m_allowedCcdPenetration);
} }
else else
{ {
world->convexSweepTest (m_convexShape, start, end, callback); world->convexSweepTest (m_convexShape, start, end, callback);
} }
if (callback.hasHit()) if (callback.hasHit())
{ {
// we moved up only a fraction of the step height // we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction; m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
} else { } else {
m_currentStepOffset = m_stepHeight; m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition; m_currentPosition = m_targetPosition;
} }
} }
void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag) void btKinematicCharacterController::updateTargetPositionBasedOnCollision (const btVector3& hitNormal, btScalar tangentMag, btScalar normalMag)
{ {
btVector3 movementDirection = m_targetPosition - m_currentPosition; btVector3 movementDirection = m_targetPosition - m_currentPosition;
btScalar movementLength = movementDirection.length(); btScalar movementLength = movementDirection.length();
if (movementLength>SIMD_EPSILON) if (movementLength>SIMD_EPSILON)
{ {
movementDirection.normalize(); movementDirection.normalize();
btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal); btVector3 reflectDir = computeReflectionDirection (movementDirection, hitNormal);
reflectDir.normalize(); reflectDir.normalize();
btVector3 parallelDir, perpindicularDir; btVector3 parallelDir, perpindicularDir;
parallelDir = parallelComponent (reflectDir, hitNormal); parallelDir = parallelComponent (reflectDir, hitNormal);
perpindicularDir = perpindicularComponent (reflectDir, hitNormal); perpindicularDir = perpindicularComponent (reflectDir, hitNormal);
m_targetPosition = m_currentPosition; m_targetPosition = m_currentPosition;
if (0)//tangentMag != 0.0) if (0)//tangentMag != 0.0)
{ {
btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength); btVector3 parComponent = parallelDir * btScalar (tangentMag*movementLength);
// printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]); // printf("parComponent=%f,%f,%f\n",parComponent[0],parComponent[1],parComponent[2]);
m_targetPosition += parComponent; m_targetPosition += parComponent;
} }
if (normalMag != 0.0) if (normalMag != 0.0)
{ {
btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength); btVector3 perpComponent = perpindicularDir * btScalar (normalMag*movementLength);
// printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]); // printf("perpComponent=%f,%f,%f\n",perpComponent[0],perpComponent[1],perpComponent[2]);
m_targetPosition += perpComponent; m_targetPosition += perpComponent;
} }
} else } else
{ {
// printf("movementLength don't normalize a zero vector\n"); // printf("movementLength don't normalize a zero vector\n");
} }
} }
void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove) void btKinematicCharacterController::stepForwardAndStrafe ( btCollisionWorld* collisionWorld, const btVector3& walkMove)
{ {
btVector3 originalDir = walkMove.normalized(); btVector3 originalDir = walkMove.normalized();
if (walkMove.length() < SIMD_EPSILON) if (walkMove.length() < SIMD_EPSILON)
{ {
originalDir.setValue(0.f,0.f,0.f); originalDir.setValue(0.f,0.f,0.f);
} }
// printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]); // printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
// phase 2: forward and strafe // phase 2: forward and strafe
btTransform start, end; btTransform start, end;
m_targetPosition = m_currentPosition + walkMove; m_targetPosition = m_currentPosition + walkMove;
start.setIdentity (); start.setIdentity ();
end.setIdentity (); end.setIdentity ();
btScalar fraction = 1.0; btScalar fraction = 1.0;
btScalar distance2 = (m_currentPosition-m_targetPosition).length2(); btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
// printf("distance2=%f\n",distance2); // printf("distance2=%f\n",distance2);
if (m_touchingContact) if (m_touchingContact)
{ {
if (originalDir.dot(m_touchingNormal) > btScalar(0.0)) if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
updateTargetPositionBasedOnCollision (m_touchingNormal); updateTargetPositionBasedOnCollision (m_touchingNormal);
} }
int maxIter = 10; int maxIter = 10;
while (fraction > btScalar(0.01) && maxIter-- > 0) while (fraction > btScalar(0.01) && maxIter-- > 0)
{ {
start.setOrigin (m_currentPosition); start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition); end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
btScalar margin = m_convexShape->getMargin(); btScalar margin = m_convexShape->getMargin();
m_convexShape->setMargin(margin + m_addedMargin); m_convexShape->setMargin(margin + m_addedMargin);
if (m_useGhostObjectSweepTest) if (m_useGhostObjectSweepTest)
{ {
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else } else
{ {
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} }
m_convexShape->setMargin(margin); m_convexShape->setMargin(margin);
fraction -= callback.m_closestHitFraction; fraction -= callback.m_closestHitFraction;
if (callback.hasHit()) if (callback.hasHit())
{ {
// we moved only a fraction // we moved only a fraction
btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length(); btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
if (hitDistance<0.f) if (hitDistance<0.f)
{ {
// printf("neg dist?\n"); // printf("neg dist?\n");
} }
/* If the distance is farther than the collision margin, move */ /* If the distance is farther than the collision margin, move */
if (hitDistance > m_addedMargin) if (hitDistance > m_addedMargin)
{ {
// printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction); // printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
} }
updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld); updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
btVector3 currentDir = m_targetPosition - m_currentPosition; btVector3 currentDir = m_targetPosition - m_currentPosition;
distance2 = currentDir.length2(); distance2 = currentDir.length2();
if (distance2 > SIMD_EPSILON) if (distance2 > SIMD_EPSILON)
{ {
currentDir.normalize(); currentDir.normalize();
/* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
if (currentDir.dot(originalDir) <= btScalar(0.0)) if (currentDir.dot(originalDir) <= btScalar(0.0))
{ {
break; break;
} }
} else } else
{ {
// printf("currentDir: don't normalize a zero vector\n"); // printf("currentDir: don't normalize a zero vector\n");
break; break;
} }
} else { } else {
// we moved whole way // we moved whole way
m_currentPosition = m_targetPosition; m_currentPosition = m_targetPosition;
} }
// if (callback.m_closestHitFraction == 0.f) // if (callback.m_closestHitFraction == 0.f)
// break; // break;
} }
} }
void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt) void btKinematicCharacterController::stepDown ( btCollisionWorld* collisionWorld, btScalar dt)
{ {
btTransform start, end; btTransform start, end;
// phase 3: down // phase 3: down
btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset; btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight; btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop); m_targetPosition -= (step_drop + gravity_drop);
start.setIdentity (); start.setIdentity ();
end.setIdentity (); end.setIdentity ();
start.setOrigin (m_currentPosition); start.setOrigin (m_currentPosition);
end.setOrigin (m_targetPosition); end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject); btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup; callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask; callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;
if (m_useGhostObjectSweepTest) if (m_useGhostObjectSweepTest)
{ {
m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} else } else
{ {
collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration); collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
} }
if (callback.hasHit()) if (callback.hasHit())
{ {
// we dropped a fraction of the height -> hit floor // we dropped a fraction of the height -> hit floor
m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction); m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
} else { } else {
// we dropped the full height // we dropped the full height
m_currentPosition = m_targetPosition; m_currentPosition = m_targetPosition;
} }
} }
void btKinematicCharacterController::reset () void btKinematicCharacterController::reset ()
{ {
} }
void btKinematicCharacterController::warp (const btVector3& origin) void btKinematicCharacterController::warp (const btVector3& origin)
{ {
btTransform xform; btTransform xform;
xform.setIdentity(); xform.setIdentity();
xform.setOrigin (origin); xform.setOrigin (origin);
m_ghostObject->setWorldTransform (xform); m_ghostObject->setWorldTransform (xform);
} }
void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld) void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{ {
int numPenetrationLoops = 0; int numPenetrationLoops = 0;
m_touchingContact = false; m_touchingContact = false;
while (recoverFromPenetration (collisionWorld)) while (recoverFromPenetration (collisionWorld))
{ {
numPenetrationLoops++; numPenetrationLoops++;
m_touchingContact = true; m_touchingContact = true;
if (numPenetrationLoops > 4) if (numPenetrationLoops > 4)
{ {
// printf("character could not recover from penetration = %d\n", numPenetrationLoops); // printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break; break;
} }
} }
m_currentPosition = m_ghostObject->getWorldTransform().getOrigin(); m_currentPosition = m_ghostObject->getWorldTransform().getOrigin();
m_targetPosition = m_currentPosition; m_targetPosition = m_currentPosition;
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]); // printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
} }
void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt) void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{ {
btTransform xform; btTransform xform;
xform = m_ghostObject->getWorldTransform (); xform = m_ghostObject->getWorldTransform ();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]); // printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed); // printf("walkSpeed=%f\n",walkSpeed);
stepUp (collisionWorld); stepUp (collisionWorld);
stepForwardAndStrafe (collisionWorld, m_walkDirection); stepForwardAndStrafe (collisionWorld, m_walkDirection);
stepDown (collisionWorld, dt); stepDown (collisionWorld, dt);
xform.setOrigin (m_currentPosition); xform.setOrigin (m_currentPosition);
m_ghostObject->setWorldTransform (xform); m_ghostObject->setWorldTransform (xform);
} }
void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed) void btKinematicCharacterController::setFallSpeed (btScalar fallSpeed)
{ {
m_fallSpeed = fallSpeed; m_fallSpeed = fallSpeed;
} }
void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed) void btKinematicCharacterController::setJumpSpeed (btScalar jumpSpeed)
{ {
m_jumpSpeed = jumpSpeed; m_jumpSpeed = jumpSpeed;
} }
void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight) void btKinematicCharacterController::setMaxJumpHeight (btScalar maxJumpHeight)
{ {
m_maxJumpHeight = maxJumpHeight; m_maxJumpHeight = maxJumpHeight;
} }
bool btKinematicCharacterController::canJump () const bool btKinematicCharacterController::canJump () const
{ {
return onGround(); return onGround();
} }
void btKinematicCharacterController::jump () void btKinematicCharacterController::jump ()
{ {
if (!canJump()) if (!canJump())
return; return;
#if 0 #if 0
currently no jumping. currently no jumping.
btTransform xform; btTransform xform;
m_rigidBody->getMotionState()->getWorldTransform (xform); m_rigidBody->getMotionState()->getWorldTransform (xform);
btVector3 up = xform.getBasis()[1]; btVector3 up = xform.getBasis()[1];
up.normalize (); up.normalize ();
btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0); btScalar magnitude = (btScalar(1.0)/m_rigidBody->getInvMass()) * btScalar(8.0);
m_rigidBody->applyCentralImpulse (up * magnitude); m_rigidBody->applyCentralImpulse (up * magnitude);
#endif #endif
} }
bool btKinematicCharacterController::onGround () const bool btKinematicCharacterController::onGround () const
{ {
return true; return true;
} }

232
src/BulletDynamics/Character/btKinematicCharacterController.h
View File

@@ -1,116 +1,116 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef KINEMATIC_CHARACTER_CONTROLLER_H #ifndef KINEMATIC_CHARACTER_CONTROLLER_H
#define KINEMATIC_CHARACTER_CONTROLLER_H #define KINEMATIC_CHARACTER_CONTROLLER_H
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "btCharacterControllerInterface.h" #include "btCharacterControllerInterface.h"
class btCollisionShape; class btCollisionShape;
class btRigidBody; class btRigidBody;
class btCollisionWorld; class btCollisionWorld;
class btCollisionDispatcher; class btCollisionDispatcher;
class btPairCachingGhostObject; class btPairCachingGhostObject;
///btKinematicCharacterController is an object that supports a sliding motion in a world. ///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. ///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. ///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
class btKinematicCharacterController : public btCharacterControllerInterface class btKinematicCharacterController : public btCharacterControllerInterface
{ {
protected: protected:
btScalar m_halfHeight; btScalar m_halfHeight;
btPairCachingGhostObject* m_ghostObject; 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 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_fallSpeed;
btScalar m_jumpSpeed; btScalar m_jumpSpeed;
btScalar m_maxJumpHeight; btScalar m_maxJumpHeight;
btScalar m_turnAngle; btScalar m_turnAngle;
btScalar m_stepHeight; btScalar m_stepHeight;
btScalar m_addedMargin;//@todo: remove this and fix the code btScalar m_addedMargin;//@todo: remove this and fix the code
///this is the desired walk direction, set by the user ///this is the desired walk direction, set by the user
btVector3 m_walkDirection; btVector3 m_walkDirection;
//some internal variables //some internal variables
btVector3 m_currentPosition; btVector3 m_currentPosition;
btScalar m_currentStepOffset; btScalar m_currentStepOffset;
btVector3 m_targetPosition; btVector3 m_targetPosition;
///keep track of the contact manifolds ///keep track of the contact manifolds
btManifoldArray m_manifoldArray; btManifoldArray m_manifoldArray;
bool m_touchingContact; bool m_touchingContact;
btVector3 m_touchingNormal; btVector3 m_touchingNormal;
bool m_useGhostObjectSweepTest; bool m_useGhostObjectSweepTest;
int m_upAxis; int m_upAxis;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal); btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal); btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal); btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
bool recoverFromPenetration (btCollisionWorld* collisionWorld); bool recoverFromPenetration (btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld); void stepUp (btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0)); void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove); void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt); void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public: public:
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1); btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~btKinematicCharacterController (); ~btKinematicCharacterController ();
void setUpAxis (int axis) void setUpAxis (int axis)
{ {
if (axis < 0) if (axis < 0)
axis = 0; axis = 0;
if (axis > 2) if (axis > 2)
axis = 2; axis = 2;
m_upAxis = axis; m_upAxis = axis;
} }
virtual void setWalkDirection(const btVector3& walkDirection) virtual void setWalkDirection(const btVector3& walkDirection)
{ {
m_walkDirection = walkDirection; m_walkDirection = walkDirection;
} }
void reset (); void reset ();
void warp (const btVector3& origin); void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld); void preStep ( btCollisionWorld* collisionWorld);
void playerStep (btCollisionWorld* collisionWorld, btScalar dt); void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
void setFallSpeed (btScalar fallSpeed); void setFallSpeed (btScalar fallSpeed);
void setJumpSpeed (btScalar jumpSpeed); void setJumpSpeed (btScalar jumpSpeed);
void setMaxJumpHeight (btScalar maxJumpHeight); void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const; bool canJump () const;
void jump (); void jump ();
btPairCachingGhostObject* getGhostObject(); btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest) void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{ {
m_useGhostObjectSweepTest = useGhostObjectSweepTest; m_useGhostObjectSweepTest = useGhostObjectSweepTest;
} }
bool onGround () const; bool onGround () const;
}; };
#endif // KINEMATIC_CHARACTER_CONTROLLER_H #endif // KINEMATIC_CHARACTER_CONTROLLER_H

1634
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

File diff suppressed because it is too large Load Diff

458
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -1,229 +1,229 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
/* /*
Added by Roman Ponomarev (rponom@gmail.com) Added by Roman Ponomarev (rponom@gmail.com)
April 04, 2008 April 04, 2008
TODO: TODO:
- add clamping od accumulated impulse to improve stability - add clamping od accumulated impulse to improve stability
- add conversion for ODE constraint solver - add conversion for ODE constraint solver
*/ */
#ifndef SLIDER_CONSTRAINT_H #ifndef SLIDER_CONSTRAINT_H
#define SLIDER_CONSTRAINT_H #define SLIDER_CONSTRAINT_H
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "btJacobianEntry.h" #include "btJacobianEntry.h"
#include "btTypedConstraint.h" #include "btTypedConstraint.h"
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
class btRigidBody; class btRigidBody;
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_SOFTNESS (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0)) #define SLIDER_CONSTRAINT_DEF_DAMPING (btScalar(1.0))
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7)) #define SLIDER_CONSTRAINT_DEF_RESTITUTION (btScalar(0.7))
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
class btSliderConstraint : public btTypedConstraint class btSliderConstraint : public btTypedConstraint
{ {
protected: protected:
///for backwards compatibility during the transition to 'getInfo/getInfo2' ///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete; bool m_useSolveConstraintObsolete;
btTransform m_frameInA; btTransform m_frameInA;
btTransform m_frameInB; btTransform m_frameInB;
// use frameA fo define limits, if true // use frameA fo define limits, if true
bool m_useLinearReferenceFrameA; bool m_useLinearReferenceFrameA;
// linear limits // linear limits
btScalar m_lowerLinLimit; btScalar m_lowerLinLimit;
btScalar m_upperLinLimit; btScalar m_upperLinLimit;
// angular limits // angular limits
btScalar m_lowerAngLimit; btScalar m_lowerAngLimit;
btScalar m_upperAngLimit; btScalar m_upperAngLimit;
// softness, restitution and damping for different cases // softness, restitution and damping for different cases
// DirLin - moving inside linear limits // DirLin - moving inside linear limits
// LimLin - hitting linear limit // LimLin - hitting linear limit
// DirAng - moving inside angular limits // DirAng - moving inside angular limits
// LimAng - hitting angular limit // LimAng - hitting angular limit
// OrthoLin, OrthoAng - against constraint axis // OrthoLin, OrthoAng - against constraint axis
btScalar m_softnessDirLin; btScalar m_softnessDirLin;
btScalar m_restitutionDirLin; btScalar m_restitutionDirLin;
btScalar m_dampingDirLin; btScalar m_dampingDirLin;
btScalar m_softnessDirAng; btScalar m_softnessDirAng;
btScalar m_restitutionDirAng; btScalar m_restitutionDirAng;
btScalar m_dampingDirAng; btScalar m_dampingDirAng;
btScalar m_softnessLimLin; btScalar m_softnessLimLin;
btScalar m_restitutionLimLin; btScalar m_restitutionLimLin;
btScalar m_dampingLimLin; btScalar m_dampingLimLin;
btScalar m_softnessLimAng; btScalar m_softnessLimAng;
btScalar m_restitutionLimAng; btScalar m_restitutionLimAng;
btScalar m_dampingLimAng; btScalar m_dampingLimAng;
btScalar m_softnessOrthoLin; btScalar m_softnessOrthoLin;
btScalar m_restitutionOrthoLin; btScalar m_restitutionOrthoLin;
btScalar m_dampingOrthoLin; btScalar m_dampingOrthoLin;
btScalar m_softnessOrthoAng; btScalar m_softnessOrthoAng;
btScalar m_restitutionOrthoAng; btScalar m_restitutionOrthoAng;
btScalar m_dampingOrthoAng; btScalar m_dampingOrthoAng;
// for interlal use // for interlal use
bool m_solveLinLim; bool m_solveLinLim;
bool m_solveAngLim; bool m_solveAngLim;
btJacobianEntry m_jacLin[3]; btJacobianEntry m_jacLin[3];
btScalar m_jacLinDiagABInv[3]; btScalar m_jacLinDiagABInv[3];
btJacobianEntry m_jacAng[3]; btJacobianEntry m_jacAng[3];
btScalar m_timeStep; btScalar m_timeStep;
btTransform m_calculatedTransformA; btTransform m_calculatedTransformA;
btTransform m_calculatedTransformB; btTransform m_calculatedTransformB;
btVector3 m_sliderAxis; btVector3 m_sliderAxis;
btVector3 m_realPivotAInW; btVector3 m_realPivotAInW;
btVector3 m_realPivotBInW; btVector3 m_realPivotBInW;
btVector3 m_projPivotInW; btVector3 m_projPivotInW;
btVector3 m_delta; btVector3 m_delta;
btVector3 m_depth; btVector3 m_depth;
btVector3 m_relPosA; btVector3 m_relPosA;
btVector3 m_relPosB; btVector3 m_relPosB;
btScalar m_linPos; btScalar m_linPos;
btScalar m_angPos; btScalar m_angPos;
btScalar m_angDepth; btScalar m_angDepth;
btScalar m_kAngle; btScalar m_kAngle;
bool m_poweredLinMotor; bool m_poweredLinMotor;
btScalar m_targetLinMotorVelocity; btScalar m_targetLinMotorVelocity;
btScalar m_maxLinMotorForce; btScalar m_maxLinMotorForce;
btScalar m_accumulatedLinMotorImpulse; btScalar m_accumulatedLinMotorImpulse;
bool m_poweredAngMotor; bool m_poweredAngMotor;
btScalar m_targetAngMotorVelocity; btScalar m_targetAngMotorVelocity;
btScalar m_maxAngMotorForce; btScalar m_maxAngMotorForce;
btScalar m_accumulatedAngMotorImpulse; btScalar m_accumulatedAngMotorImpulse;
//------------------------ //------------------------
void initParams(); void initParams();
public: public:
// constructors // constructors
btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btSliderConstraint(); btSliderConstraint();
// overrides // overrides
virtual void buildJacobian(); virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info); virtual void getInfo2 (btConstraintInfo2* info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
// access // access
const btRigidBody& getRigidBodyA() const { return m_rbA; } const btRigidBody& getRigidBodyA() const { return m_rbA; }
const btRigidBody& getRigidBodyB() const { return m_rbB; } const btRigidBody& getRigidBodyB() const { return m_rbB; }
const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; } const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; } const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
const btTransform & getFrameOffsetA() const { return m_frameInA; } const btTransform & getFrameOffsetA() const { return m_frameInA; }
const btTransform & getFrameOffsetB() const { return m_frameInB; } const btTransform & getFrameOffsetB() const { return m_frameInB; }
btTransform & getFrameOffsetA() { return m_frameInA; } btTransform & getFrameOffsetA() { return m_frameInA; }
btTransform & getFrameOffsetB() { return m_frameInB; } btTransform & getFrameOffsetB() { return m_frameInB; }
btScalar getLowerLinLimit() { return m_lowerLinLimit; } btScalar getLowerLinLimit() { return m_lowerLinLimit; }
void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; } void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
btScalar getUpperLinLimit() { return m_upperLinLimit; } btScalar getUpperLinLimit() { return m_upperLinLimit; }
void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; } void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
btScalar getLowerAngLimit() { return m_lowerAngLimit; } btScalar getLowerAngLimit() { return m_lowerAngLimit; }
void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; } void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
btScalar getUpperAngLimit() { return m_upperAngLimit; } btScalar getUpperAngLimit() { return m_upperAngLimit; }
void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; } void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; } bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
btScalar getSoftnessDirLin() { return m_softnessDirLin; } btScalar getSoftnessDirLin() { return m_softnessDirLin; }
btScalar getRestitutionDirLin() { return m_restitutionDirLin; } btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
btScalar getDampingDirLin() { return m_dampingDirLin ; } btScalar getDampingDirLin() { return m_dampingDirLin ; }
btScalar getSoftnessDirAng() { return m_softnessDirAng; } btScalar getSoftnessDirAng() { return m_softnessDirAng; }
btScalar getRestitutionDirAng() { return m_restitutionDirAng; } btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
btScalar getDampingDirAng() { return m_dampingDirAng; } btScalar getDampingDirAng() { return m_dampingDirAng; }
btScalar getSoftnessLimLin() { return m_softnessLimLin; } btScalar getSoftnessLimLin() { return m_softnessLimLin; }
btScalar getRestitutionLimLin() { return m_restitutionLimLin; } btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
btScalar getDampingLimLin() { return m_dampingLimLin; } btScalar getDampingLimLin() { return m_dampingLimLin; }
btScalar getSoftnessLimAng() { return m_softnessLimAng; } btScalar getSoftnessLimAng() { return m_softnessLimAng; }
btScalar getRestitutionLimAng() { return m_restitutionLimAng; } btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
btScalar getDampingLimAng() { return m_dampingLimAng; } btScalar getDampingLimAng() { return m_dampingLimAng; }
btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; } btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; } btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
btScalar getDampingOrthoLin() { return m_dampingOrthoLin; } btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; } btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; } btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
btScalar getDampingOrthoAng() { return m_dampingOrthoAng; } btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; } void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; } void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; } void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; } void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; } void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; } void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; } void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; } void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; } void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; } void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; } void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; } void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; } void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; } void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; } void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; } void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; } void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; } void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; } void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
bool getPoweredLinMotor() { return m_poweredLinMotor; } bool getPoweredLinMotor() { return m_poweredLinMotor; }
void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; } void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; } btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; } void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; } btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; } void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
bool getPoweredAngMotor() { return m_poweredAngMotor; } bool getPoweredAngMotor() { return m_poweredAngMotor; }
void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; } void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
btScalar getLinearPos() { return m_linPos; } btScalar getLinearPos() { return m_linPos; }
// access for ODE solver // access for ODE solver
bool getSolveLinLimit() { return m_solveLinLim; } bool getSolveLinLimit() { return m_solveLinLim; }
btScalar getLinDepth() { return m_depth[0]; } btScalar getLinDepth() { return m_depth[0]; }
bool getSolveAngLimit() { return m_solveAngLim; } bool getSolveAngLimit() { return m_solveAngLim; }
btScalar getAngDepth() { return m_angDepth; } btScalar getAngDepth() { return m_angDepth; }
// internal // internal
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
// shared code used by ODE solver // shared code used by ODE solver
void calculateTransforms(void); void calculateTransforms(void);
void testLinLimits(void); void testLinLimits(void);
void testLinLimits2(btConstraintInfo2* info); void testLinLimits2(btConstraintInfo2* info);
void testAngLimits(void); void testAngLimits(void);
// access for PE Solver // access for PE Solver
btVector3 getAncorInA(void); btVector3 getAncorInA(void);
btVector3 getAncorInB(void); btVector3 getAncorInB(void);
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
#endif //SLIDER_CONSTRAINT_H #endif //SLIDER_CONSTRAINT_H

358
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -1,179 +1,179 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_SOLVER_BODY_H #ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H #define BT_SOLVER_BODY_H
class btRigidBody; class btRigidBody;
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.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 ///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 #ifdef BT_USE_SSE
#define USE_SIMD 1 #define USE_SIMD 1
#endif // #endif //
#ifdef USE_SIMD #ifdef USE_SIMD
struct btSimdScalar struct btSimdScalar
{ {
SIMD_FORCE_INLINE btSimdScalar() SIMD_FORCE_INLINE btSimdScalar()
{ {
} }
SIMD_FORCE_INLINE btSimdScalar(float fl) SIMD_FORCE_INLINE btSimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl)) :m_vec128 (_mm_set1_ps(fl))
{ {
} }
SIMD_FORCE_INLINE btSimdScalar(__m128 v128) SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
:m_vec128(v128) :m_vec128(v128)
{ {
} }
union union
{ {
__m128 m_vec128; __m128 m_vec128;
float m_floats[4]; float m_floats[4];
int m_ints[4]; int m_ints[4];
btScalar m_unusedPadding; btScalar m_unusedPadding;
}; };
SIMD_FORCE_INLINE __m128 get128() SIMD_FORCE_INLINE __m128 get128()
{ {
return m_vec128; return m_vec128;
} }
SIMD_FORCE_INLINE const __m128 get128() const SIMD_FORCE_INLINE const __m128 get128() const
{ {
return m_vec128; return m_vec128;
} }
SIMD_FORCE_INLINE void set128(__m128 v128) SIMD_FORCE_INLINE void set128(__m128 v128)
{ {
m_vec128 = v128; m_vec128 = v128;
} }
SIMD_FORCE_INLINE operator __m128() SIMD_FORCE_INLINE operator __m128()
{ {
return m_vec128; return m_vec128;
} }
SIMD_FORCE_INLINE operator const __m128() const SIMD_FORCE_INLINE operator const __m128() const
{ {
return m_vec128; return m_vec128;
} }
SIMD_FORCE_INLINE operator float() const SIMD_FORCE_INLINE operator float() const
{ {
return m_floats[0]; return m_floats[0];
} }
}; };
///@brief Return the elementwise product of two btSimdScalar ///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2) operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{ {
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128())); return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
} }
///@brief Return the elementwise product of two btSimdScalar ///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2) operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{ {
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128())); return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
} }
#else #else
#define btSimdScalar btScalar #define btSimdScalar btScalar
#endif #endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_deltaLinearVelocity; btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity; btVector3 m_deltaAngularVelocity;
btScalar m_angularFactor; btScalar m_angularFactor;
btScalar m_invMass; btScalar m_invMass;
btScalar m_friction; btScalar m_friction;
btRigidBody* m_originalBody; btRigidBody* m_originalBody;
btVector3 m_pushVelocity; btVector3 m_pushVelocity;
//btVector3 m_turnVelocity; //btVector3 m_turnVelocity;
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{ {
if (m_originalBody) if (m_originalBody)
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
else else
velocity.setValue(0,0,0); velocity.setValue(0,0,0);
} }
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{ {
if (m_originalBody) if (m_originalBody)
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
else else
angVel.setValue(0,0,0); angVel.setValue(0,0,0);
} }
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position //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) SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{ {
//if (m_invMass) //if (m_invMass)
{ {
m_deltaLinearVelocity += linearComponent*impulseMagnitude; m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
} }
} }
/* /*
void writebackVelocity() void writebackVelocity()
{ {
if (m_invMass) if (m_invMass)
{ {
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1); //m_originalBody->setCompanionId(-1);
} }
} }
*/ */
void writebackVelocity(btScalar timeStep=0) void writebackVelocity(btScalar timeStep=0)
{ {
if (m_invMass) if (m_invMass)
{ {
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity); m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1); //m_originalBody->setCompanionId(-1);
} }
} }
}; };
#endif //BT_SOLVER_BODY_H #endif //BT_SOLVER_BODY_H

2
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
static btRigidBody s_fixed(0, 0,0); 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) btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
:m_userConstraintType(-1), :m_userConstraintType(-1),

2856
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

File diff suppressed because it is too large Load Diff

352
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -1,176 +1,176 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_DISCRETE_DYNAMICS_WORLD_H #ifndef BT_DISCRETE_DYNAMICS_WORLD_H
#define BT_DISCRETE_DYNAMICS_WORLD_H #define BT_DISCRETE_DYNAMICS_WORLD_H
#include "btDynamicsWorld.h" #include "btDynamicsWorld.h"
class btDispatcher; class btDispatcher;
class btOverlappingPairCache; class btOverlappingPairCache;
class btConstraintSolver; class btConstraintSolver;
class btSimulationIslandManager; class btSimulationIslandManager;
class btTypedConstraint; class btTypedConstraint;
class btRaycastVehicle; class btRaycastVehicle;
class btCharacterControllerInterface; class btCharacterControllerInterface;
class btIDebugDraw; class btIDebugDraw;
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
///btDiscreteDynamicsWorld provides discrete rigid body simulation ///btDiscreteDynamicsWorld provides discrete rigid body simulation
///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
class btDiscreteDynamicsWorld : public btDynamicsWorld class btDiscreteDynamicsWorld : public btDynamicsWorld
{ {
protected: protected:
btConstraintSolver* m_constraintSolver; btConstraintSolver* m_constraintSolver;
btSimulationIslandManager* m_islandManager; btSimulationIslandManager* m_islandManager;
btAlignedObjectArray<btTypedConstraint*> m_constraints; btAlignedObjectArray<btTypedConstraint*> m_constraints;
btVector3 m_gravity; btVector3 m_gravity;
//for variable timesteps //for variable timesteps
btScalar m_localTime; btScalar m_localTime;
//for variable timesteps //for variable timesteps
bool m_ownsIslandManager; bool m_ownsIslandManager;
bool m_ownsConstraintSolver; bool m_ownsConstraintSolver;
btAlignedObjectArray<btRaycastVehicle*> m_vehicles; btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
btAlignedObjectArray<btCharacterControllerInterface*> m_characters; btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
int m_profileTimings; int m_profileTimings;
virtual void predictUnconstraintMotion(btScalar timeStep); virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void integrateTransforms(btScalar timeStep); virtual void integrateTransforms(btScalar timeStep);
virtual void calculateSimulationIslands(); virtual void calculateSimulationIslands();
virtual void solveConstraints(btContactSolverInfo& solverInfo); virtual void solveConstraints(btContactSolverInfo& solverInfo);
void updateActivationState(btScalar timeStep); void updateActivationState(btScalar timeStep);
void updateVehicles(btScalar timeStep); void updateVehicles(btScalar timeStep);
void updateCharacters(btScalar timeStep); void updateCharacters(btScalar timeStep);
void startProfiling(btScalar timeStep); void startProfiling(btScalar timeStep);
virtual void internalSingleStepSimulation( btScalar timeStep); virtual void internalSingleStepSimulation( btScalar timeStep);
virtual void saveKinematicState(btScalar timeStep); virtual void saveKinematicState(btScalar timeStep);
void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color);
public: public:
///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
virtual ~btDiscreteDynamicsWorld(); virtual ~btDiscreteDynamicsWorld();
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's ///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 int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
virtual void synchronizeMotionStates(); virtual void synchronizeMotionStates();
///this can be useful to synchronize a single rigid body -> graphics object ///this can be useful to synchronize a single rigid body -> graphics object
void synchronizeSingleMotionState(btRigidBody* body); void synchronizeSingleMotionState(btRigidBody* body);
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
virtual void removeConstraint(btTypedConstraint* constraint); virtual void removeConstraint(btTypedConstraint* constraint);
virtual void addVehicle(btRaycastVehicle* vehicle); virtual void addVehicle(btRaycastVehicle* vehicle);
virtual void removeVehicle(btRaycastVehicle* vehicle); virtual void removeVehicle(btRaycastVehicle* vehicle);
virtual void addCharacter(btCharacterControllerInterface* character); virtual void addCharacter(btCharacterControllerInterface* character);
virtual void removeCharacter(btCharacterControllerInterface* character); virtual void removeCharacter(btCharacterControllerInterface* character);
btSimulationIslandManager* getSimulationIslandManager() btSimulationIslandManager* getSimulationIslandManager()
{ {
return m_islandManager; return m_islandManager;
} }
const btSimulationIslandManager* getSimulationIslandManager() const const btSimulationIslandManager* getSimulationIslandManager() const
{ {
return m_islandManager; return m_islandManager;
} }
btCollisionWorld* getCollisionWorld() btCollisionWorld* getCollisionWorld()
{ {
return this; return this;
} }
virtual void setGravity(const btVector3& gravity); virtual void setGravity(const btVector3& gravity);
virtual btVector3 getGravity () const; virtual btVector3 getGravity () const;
virtual void addRigidBody(btRigidBody* body); virtual void addRigidBody(btRigidBody* body);
virtual void addRigidBody(btRigidBody* body, short group, short mask); virtual void addRigidBody(btRigidBody* body, short group, short mask);
virtual void removeRigidBody(btRigidBody* body); virtual void removeRigidBody(btRigidBody* body);
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
void debugDrawConstraint(btTypedConstraint* constraint); void debugDrawConstraint(btTypedConstraint* constraint);
virtual void debugDrawWorld(); virtual void debugDrawWorld();
virtual void setConstraintSolver(btConstraintSolver* solver); virtual void setConstraintSolver(btConstraintSolver* solver);
virtual btConstraintSolver* getConstraintSolver(); virtual btConstraintSolver* getConstraintSolver();
virtual int getNumConstraints() const; virtual int getNumConstraints() const;
virtual btTypedConstraint* getConstraint(int index) ; virtual btTypedConstraint* getConstraint(int index) ;
virtual const btTypedConstraint* getConstraint(int index) const; virtual const btTypedConstraint* getConstraint(int index) const;
virtual btDynamicsWorldType getWorldType() const virtual btDynamicsWorldType getWorldType() const
{ {
return BT_DISCRETE_DYNAMICS_WORLD; return BT_DISCRETE_DYNAMICS_WORLD;
} }
///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
virtual void clearForces(); virtual void clearForces();
///apply gravity, call this once per timestep ///apply gravity, call this once per timestep
virtual void applyGravity(); virtual void applyGravity();
virtual void setNumTasks(int numTasks) virtual void setNumTasks(int numTasks)
{ {
(void) numTasks; (void) numTasks;
} }
}; };
#endif //BT_DISCRETE_DYNAMICS_WORLD_H #endif //BT_DISCRETE_DYNAMICS_WORLD_H

272
src/BulletDynamics/Dynamics/btDynamicsWorld.h
View File

@@ -1,136 +1,136 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef BT_DYNAMICS_WORLD_H #ifndef BT_DYNAMICS_WORLD_H
#define BT_DYNAMICS_WORLD_H #define BT_DYNAMICS_WORLD_H
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class btTypedConstraint; class btTypedConstraint;
class btRaycastVehicle; class btRaycastVehicle;
class btConstraintSolver; class btConstraintSolver;
class btDynamicsWorld; class btDynamicsWorld;
class btCharacterControllerInterface; class btCharacterControllerInterface;
/// Type for the callback for each tick /// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
enum btDynamicsWorldType enum btDynamicsWorldType
{ {
BT_SIMPLE_DYNAMICS_WORLD=1, BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2, BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3 BT_CONTINUOUS_DYNAMICS_WORLD=3
}; };
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
class btDynamicsWorld : public btCollisionWorld class btDynamicsWorld : public btCollisionWorld
{ {
protected: protected:
btInternalTickCallback m_internalTickCallback; btInternalTickCallback m_internalTickCallback;
void* m_worldUserInfo; void* m_worldUserInfo;
btContactSolverInfo m_solverInfo; btContactSolverInfo m_solverInfo;
public: public:
btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0) :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
{ {
} }
virtual ~btDynamicsWorld() virtual ~btDynamicsWorld()
{ {
} }
///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. ///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'. ///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. ///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 int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
virtual void debugDrawWorld() = 0; virtual void debugDrawWorld() = 0;
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false)
{ {
(void)constraint; (void)disableCollisionsBetweenLinkedBodies; (void)constraint; (void)disableCollisionsBetweenLinkedBodies;
} }
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;}
virtual void removeCharacter(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 //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 //existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0; virtual void setGravity(const btVector3& gravity) = 0;
virtual btVector3 getGravity () const = 0; virtual btVector3 getGravity () const = 0;
virtual void synchronizeMotionStates() = 0; virtual void synchronizeMotionStates() = 0;
virtual void addRigidBody(btRigidBody* body) = 0; virtual void addRigidBody(btRigidBody* body) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0; virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0; virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
virtual btConstraintSolver* getConstraintSolver() = 0; virtual btConstraintSolver* getConstraintSolver() = 0;
virtual int getNumConstraints() const { return 0; } virtual int getNumConstraints() const { return 0; }
virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
virtual btDynamicsWorldType getWorldType() const=0; virtual btDynamicsWorldType getWorldType() const=0;
virtual void clearForces() = 0; virtual void clearForces() = 0;
/// Set the callback for when an internal tick (simulation substep) happens, optional user info /// Set the callback for when an internal tick (simulation substep) happens, optional user info
void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0) void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
{ {
m_internalTickCallback = cb; m_internalTickCallback = cb;
m_worldUserInfo = worldUserInfo; m_worldUserInfo = worldUserInfo;
} }
void setWorldUserInfo(void* worldUserInfo) void setWorldUserInfo(void* worldUserInfo)
{ {
m_worldUserInfo = worldUserInfo; m_worldUserInfo = worldUserInfo;
} }
void* getWorldUserInfo() const void* getWorldUserInfo() const
{ {
return m_worldUserInfo; return m_worldUserInfo;
} }
btContactSolverInfo& getSolverInfo() btContactSolverInfo& getSolverInfo()
{ {
return m_solverInfo; return m_solverInfo;
} }
}; };
#endif //BT_DYNAMICS_WORLD_H #endif //BT_DYNAMICS_WORLD_H

486
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
View File

@@ -1,243 +1,243 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btSimpleDynamicsWorld.h" #include "btSimpleDynamicsWorld.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
/* /*
Make sure this dummy function never changes so that it Make sure this dummy function never changes so that it
can be used by probes that are checking whether the can be used by probes that are checking whether the
library is actually installed. library is actually installed.
*/ */
extern "C" extern "C"
{ {
void btBulletDynamicsProbe (); void btBulletDynamicsProbe ();
void btBulletDynamicsProbe () {} void btBulletDynamicsProbe () {}
} }
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
m_constraintSolver(constraintSolver), m_constraintSolver(constraintSolver),
m_ownsConstraintSolver(false), m_ownsConstraintSolver(false),
m_gravity(0,0,-10) m_gravity(0,0,-10)
{ {
} }
btSimpleDynamicsWorld::~btSimpleDynamicsWorld() btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
{ {
if (m_ownsConstraintSolver) if (m_ownsConstraintSolver)
btAlignedFree( m_constraintSolver); btAlignedFree( m_constraintSolver);
} }
int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
{ {
(void)fixedTimeStep; (void)fixedTimeStep;
(void)maxSubSteps; (void)maxSubSteps;
///apply gravity, predict motion ///apply gravity, predict motion
predictUnconstraintMotion(timeStep); predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo(); btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0; dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer(); dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection ///perform collision detection
performDiscreteCollisionDetection(); performDiscreteCollisionDetection();
///solve contact constraints ///solve contact constraints
int numManifolds = m_dispatcher1->getNumManifolds(); int numManifolds = m_dispatcher1->getNumManifolds();
if (numManifolds) if (numManifolds)
{ {
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
btContactSolverInfo infoGlobal; btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep; infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0,numManifolds); m_constraintSolver->prepareSolve(0,numManifolds);
m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
} }
///integrate transforms ///integrate transforms
integrateTransforms(timeStep); integrateTransforms(timeStep);
updateAabbs(); updateAabbs();
synchronizeMotionStates(); synchronizeMotionStates();
clearForces(); clearForces();
return 1; return 1;
} }
void btSimpleDynamicsWorld::clearForces() void btSimpleDynamicsWorld::clearForces()
{ {
///@todo: iterate over awake simulation islands! ///@todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
body->clearForces(); body->clearForces();
} }
} }
} }
void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) void btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
{ {
m_gravity = gravity; m_gravity = gravity;
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
body->setGravity(gravity); body->setGravity(gravity);
} }
} }
} }
btVector3 btSimpleDynamicsWorld::getGravity () const btVector3 btSimpleDynamicsWorld::getGravity () const
{ {
return m_gravity; return m_gravity;
} }
void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
{ {
removeCollisionObject(body); removeCollisionObject(body);
} }
void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
{ {
body->setGravity(m_gravity); body->setGravity(m_gravity);
if (body->getCollisionShape()) if (body->getCollisionShape())
{ {
addCollisionObject(body); addCollisionObject(body);
} }
} }
void btSimpleDynamicsWorld::updateAabbs() void btSimpleDynamicsWorld::updateAabbs()
{ {
btTransform predictedTrans; btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
if (body->isActive() && (!body->isStaticObject())) if (body->isActive() && (!body->isStaticObject()))
{ {
btVector3 minAabb,maxAabb; btVector3 minAabb,maxAabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btBroadphaseInterface* bp = getBroadphase(); btBroadphaseInterface* bp = getBroadphase();
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
} }
} }
} }
} }
void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
{ {
btTransform predictedTrans; btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
if (body->isActive() && (!body->isStaticObject())) if (body->isActive() && (!body->isStaticObject()))
{ {
body->predictIntegratedTransform(timeStep, predictedTrans); body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans); body->proceedToTransform( predictedTrans);
} }
} }
} }
} }
void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{ {
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body) if (body)
{ {
if (!body->isStaticObject()) if (!body->isStaticObject())
{ {
if (body->isActive()) if (body->isActive())
{ {
body->applyGravity(); body->applyGravity();
body->integrateVelocities( timeStep); body->integrateVelocities( timeStep);
body->applyDamping(timeStep); body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
} }
} }
} }
} }
} }
void btSimpleDynamicsWorld::synchronizeMotionStates() void btSimpleDynamicsWorld::synchronizeMotionStates()
{ {
///@todo: iterate over awake simulation islands! ///@todo: iterate over awake simulation islands!
for ( int i=0;i<m_collisionObjects.size();i++) for ( int i=0;i<m_collisionObjects.size();i++)
{ {
btCollisionObject* colObj = m_collisionObjects[i]; btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj); btRigidBody* body = btRigidBody::upcast(colObj);
if (body && body->getMotionState()) if (body && body->getMotionState())
{ {
if (body->getActivationState() != ISLAND_SLEEPING) if (body->getActivationState() != ISLAND_SLEEPING)
{ {
body->getMotionState()->setWorldTransform(body->getWorldTransform()); body->getMotionState()->setWorldTransform(body->getWorldTransform());
} }
} }
} }
} }
void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
{ {
if (m_ownsConstraintSolver) if (m_ownsConstraintSolver)
{ {
btAlignedFree(m_constraintSolver); btAlignedFree(m_constraintSolver);
} }
m_ownsConstraintSolver = false; m_ownsConstraintSolver = false;
m_constraintSolver = solver; m_constraintSolver = solver;
} }
btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
{ {
return m_constraintSolver; return m_constraintSolver;
} }

1458
src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
View File

File diff suppressed because it is too large Load Diff

112
src/BulletDynamics/Vehicle/btWheelInfo.cpp
View File

@@ -1,56 +1,56 @@
/* /*
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
* *
* Permission to use, copy, modify, distribute and sell this software * Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee, * and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies. * provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability * Erwin Coumans makes no representations about the suitability
* of this software for any purpose. * of this software for any purpose.
* It is provided "as is" without express or implied warranty. * It is provided "as is" without express or implied warranty.
*/ */
#include "btWheelInfo.h" #include "btWheelInfo.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity #include "BulletDynamics/Dynamics/btRigidBody.h" // for pointvelocity
btScalar btWheelInfo::getSuspensionRestLength() const btScalar btWheelInfo::getSuspensionRestLength() const
{ {
return m_suspensionRestLength1; return m_suspensionRestLength1;
} }
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo)
{ {
(void)raycastInfo; (void)raycastInfo;
if (m_raycastInfo.m_isInContact) if (m_raycastInfo.m_isInContact)
{ {
btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
btVector3 chassis_velocity_at_contactPoint; btVector3 chassis_velocity_at_contactPoint;
btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
if ( project >= btScalar(-0.1)) if ( project >= btScalar(-0.1))
{ {
m_suspensionRelativeVelocity = btScalar(0.0); m_suspensionRelativeVelocity = btScalar(0.0);
m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1);
} }
else else
{ {
btScalar inv = btScalar(-1.) / project; btScalar inv = btScalar(-1.) / project;
m_suspensionRelativeVelocity = projVel * inv; m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv; m_clippedInvContactDotSuspension = inv;
} }
} }
else // Not in contact : position wheel in a nice (rest length) position else // Not in contact : position wheel in a nice (rest length) position
{ {
m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
m_suspensionRelativeVelocity = btScalar(0.0); m_suspensionRelativeVelocity = btScalar(0.0);
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
m_clippedInvContactDotSuspension = btScalar(1.0); m_clippedInvContactDotSuspension = btScalar(1.0);
} }
} }

186
src/BulletMultiThreaded/SequentialThreadSupport.cpp
View File

@@ -1,93 +1,93 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SequentialThreadSupport.h" #include "SequentialThreadSupport.h"
#include "SpuCollisionTaskProcess.h" #include "SpuCollisionTaskProcess.h"
#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" #include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo) SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo)
{ {
startThreads(threadConstructionInfo); startThreads(threadConstructionInfo);
} }
///cleanup/shutdown Libspe2 ///cleanup/shutdown Libspe2
SequentialThreadSupport::~SequentialThreadSupport() SequentialThreadSupport::~SequentialThreadSupport()
{ {
stopSPU(); stopSPU();
} }
#include <stdio.h> #include <stdio.h>
///send messages to SPUs ///send messages to SPUs
void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId)
{ {
switch (uiCommand) switch (uiCommand)
{ {
case CMD_GATHER_AND_PROCESS_PAIRLIST: case CMD_GATHER_AND_PROCESS_PAIRLIST:
{ {
btSpuStatus& spuStatus = m_activeSpuStatus[0]; btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr=(void*)uiArgument0; spuStatus.m_userPtr=(void*)uiArgument0;
spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
} }
break; break;
default: default:
{ {
///not implemented ///not implemented
btAssert(0 && "Not implemented"); btAssert(0 && "Not implemented");
} }
}; };
} }
///check for messages from SPUs ///check for messages from SPUs
void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1) void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
{ {
btAssert(m_activeSpuStatus.size()); btAssert(m_activeSpuStatus.size());
btSpuStatus& spuStatus = m_activeSpuStatus[0]; btSpuStatus& spuStatus = m_activeSpuStatus[0];
*puiArgument0 = spuStatus.m_taskId; *puiArgument0 = spuStatus.m_taskId;
*puiArgument1 = spuStatus.m_status; *puiArgument1 = spuStatus.m_status;
} }
void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo) void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo)
{ {
m_activeSpuStatus.resize(1); m_activeSpuStatus.resize(1);
printf("STS: Not starting any threads\n"); printf("STS: Not starting any threads\n");
btSpuStatus& spuStatus = m_activeSpuStatus[0]; btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr = 0; spuStatus.m_userPtr = 0;
spuStatus.m_taskId = 0; spuStatus.m_taskId = 0;
spuStatus.m_commandId = 0; spuStatus.m_commandId = 0;
spuStatus.m_status = 0; spuStatus.m_status = 0;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc(); spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName); printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName);
} }
void SequentialThreadSupport::startSPU() void SequentialThreadSupport::startSPU()
{ {
} }
void SequentialThreadSupport::stopSPU() void SequentialThreadSupport::stopSPU()
{ {
m_activeSpuStatus.clear(); m_activeSpuStatus.clear();
} }
void SequentialThreadSupport::setNumTasks(int numTasks) void SequentialThreadSupport::setNumTasks(int numTasks)
{ {
printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks); printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks);
} }

174
src/BulletMultiThreaded/SequentialThreadSupport.h
View File

@@ -1,87 +1,87 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h" #include "PlatformDefinitions.h"
#ifndef SEQUENTIAL_THREAD_SUPPORT_H #ifndef SEQUENTIAL_THREAD_SUPPORT_H
#define SEQUENTIAL_THREAD_SUPPORT_H #define SEQUENTIAL_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h" #include "btThreadSupportInterface.h"
typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory); typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*SequentiallsMemorySetupFunc)(); typedef void* (*SequentiallsMemorySetupFunc)();
///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface ///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface
///This is useful for debugging and porting SPU Tasks to other platforms. ///This is useful for debugging and porting SPU Tasks to other platforms.
class SequentialThreadSupport : public btThreadSupportInterface class SequentialThreadSupport : public btThreadSupportInterface
{ {
public: public:
struct btSpuStatus struct btSpuStatus
{ {
uint32_t m_taskId; uint32_t m_taskId;
uint32_t m_commandId; uint32_t m_commandId;
uint32_t m_status; uint32_t m_status;
SequentialThreadFunc m_userThreadFunc; SequentialThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc
}; };
private: private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus; btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles; btAlignedObjectArray<void*> m_completeHandles;
public: public:
struct SequentialThreadConstructionInfo struct SequentialThreadConstructionInfo
{ {
SequentialThreadConstructionInfo (char* uniqueName, SequentialThreadConstructionInfo (char* uniqueName,
SequentialThreadFunc userThreadFunc, SequentialThreadFunc userThreadFunc,
SequentiallsMemorySetupFunc lsMemoryFunc SequentiallsMemorySetupFunc lsMemoryFunc
) )
:m_uniqueName(uniqueName), :m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc), m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc) m_lsMemoryFunc(lsMemoryFunc)
{ {
} }
char* m_uniqueName; char* m_uniqueName;
SequentialThreadFunc m_userThreadFunc; SequentialThreadFunc m_userThreadFunc;
SequentiallsMemorySetupFunc m_lsMemoryFunc; SequentiallsMemorySetupFunc m_lsMemoryFunc;
}; };
SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo); SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo);
virtual ~SequentialThreadSupport(); virtual ~SequentialThreadSupport();
void startThreads(SequentialThreadConstructionInfo& threadInfo); void startThreads(SequentialThreadConstructionInfo& threadInfo);
///send messages to SPUs ///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs ///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); 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) ///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(); virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks ///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU(); virtual void stopSPU();
virtual void setNumTasks(int numTasks); virtual void setNumTasks(int numTasks);
}; };
#endif //SEQUENTIAL_THREAD_SUPPORT_H #endif //SEQUENTIAL_THREAD_SUPPORT_H

138
src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp
View File

@@ -1,69 +1,69 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuContactManifoldCollisionAlgorithm.h" #include "SpuContactManifoldCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
btAssert(0); btAssert(0);
} }
btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
btAssert(0); btAssert(0);
return 1.f; return 1.f;
} }
#ifndef __SPU__ #ifndef __SPU__
SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1) SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
:btCollisionAlgorithm(ci) :btCollisionAlgorithm(ci)
#ifdef USE_SEPDISTANCE_UTIL #ifdef USE_SEPDISTANCE_UTIL
,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc()) ,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc())
#endif //USE_SEPDISTANCE_UTIL #endif //USE_SEPDISTANCE_UTIL
{ {
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_shapeType0 = body0->getCollisionShape()->getShapeType(); m_shapeType0 = body0->getCollisionShape()->getShapeType();
m_shapeType1 = body1->getCollisionShape()->getShapeType(); m_shapeType1 = body1->getCollisionShape()->getShapeType();
m_collisionMargin0 = body0->getCollisionShape()->getMargin(); m_collisionMargin0 = body0->getCollisionShape()->getMargin();
m_collisionMargin1 = body1->getCollisionShape()->getMargin(); m_collisionMargin1 = body1->getCollisionShape()->getMargin();
m_collisionObject0 = body0; m_collisionObject0 = body0;
m_collisionObject1 = body1; m_collisionObject1 = body1;
if (body0->getCollisionShape()->isPolyhedral()) if (body0->getCollisionShape()->isPolyhedral())
{ {
btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape(); btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape();
m_shapeDimensions0 = convex0->getImplicitShapeDimensions(); m_shapeDimensions0 = convex0->getImplicitShapeDimensions();
} }
if (body1->getCollisionShape()->isPolyhedral()) if (body1->getCollisionShape()->isPolyhedral())
{ {
btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape(); btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape();
m_shapeDimensions1 = convex1->getImplicitShapeDimensions(); m_shapeDimensions1 = convex1->getImplicitShapeDimensions();
} }
} }
#endif //__SPU__ #endif //__SPU__
SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm() SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm()
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr); m_dispatcher->releaseManifold(m_manifoldPtr);
} }

240
src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h
View File

@@ -1,120 +1,120 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H #ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H #define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
class btPersistentManifold; class btPersistentManifold;
//#define USE_SEPDISTANCE_UTIL 1 //#define USE_SEPDISTANCE_UTIL 1
/// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU. /// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU.
ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
{ {
btVector3 m_shapeDimensions0; btVector3 m_shapeDimensions0;
btVector3 m_shapeDimensions1; btVector3 m_shapeDimensions1;
btPersistentManifold* m_manifoldPtr; btPersistentManifold* m_manifoldPtr;
int m_shapeType0; int m_shapeType0;
int m_shapeType1; int m_shapeType1;
float m_collisionMargin0; float m_collisionMargin0;
float m_collisionMargin1; float m_collisionMargin1;
btCollisionObject* m_collisionObject0; btCollisionObject* m_collisionObject0;
btCollisionObject* m_collisionObject1; btCollisionObject* m_collisionObject1;
public: public:
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); 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 btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
#ifdef USE_SEPDISTANCE_UTIL #ifdef USE_SEPDISTANCE_UTIL
btConvexSeparatingDistanceUtil m_sepDistance; btConvexSeparatingDistanceUtil m_sepDistance;
#endif //USE_SEPDISTANCE_UTIL #endif //USE_SEPDISTANCE_UTIL
virtual ~SpuContactManifoldCollisionAlgorithm(); virtual ~SpuContactManifoldCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
if (m_manifoldPtr) if (m_manifoldPtr)
manifoldArray.push_back(m_manifoldPtr); manifoldArray.push_back(m_manifoldPtr);
} }
btPersistentManifold* getContactManifoldPtr() btPersistentManifold* getContactManifoldPtr()
{ {
return m_manifoldPtr; return m_manifoldPtr;
} }
btCollisionObject* getCollisionObject0() btCollisionObject* getCollisionObject0()
{ {
return m_collisionObject0; return m_collisionObject0;
} }
btCollisionObject* getCollisionObject1() btCollisionObject* getCollisionObject1()
{ {
return m_collisionObject1; return m_collisionObject1;
} }
int getShapeType0() const int getShapeType0() const
{ {
return m_shapeType0; return m_shapeType0;
} }
int getShapeType1() const int getShapeType1() const
{ {
return m_shapeType1; return m_shapeType1;
} }
float getCollisionMargin0() const float getCollisionMargin0() const
{ {
return m_collisionMargin0; return m_collisionMargin0;
} }
float getCollisionMargin1() const float getCollisionMargin1() const
{ {
return m_collisionMargin1; return m_collisionMargin1;
} }
const btVector3& getShapeDimensions0() const const btVector3& getShapeDimensions0() const
{ {
return m_shapeDimensions0; return m_shapeDimensions0;
} }
const btVector3& getShapeDimensions1() const const btVector3& getShapeDimensions1() const
{ {
return m_shapeDimensions1; return m_shapeDimensions1;
} }
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1); return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
} }
}; };
}; };
#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H #endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H

418
src/BulletMultiThreaded/SpuFakeDma.cpp
View File

@@ -1,209 +1,209 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuFakeDma.h" #include "SpuFakeDma.h"
#include <LinearMath/btScalar.h> //for btAssert #include <LinearMath/btScalar.h> //for btAssert
//Disabling memcpy sometimes helps debugging DMA //Disabling memcpy sometimes helps debugging DMA
#define USE_MEMCPY 1 #define USE_MEMCPY 1
#ifdef USE_MEMCPY #ifdef USE_MEMCPY
#endif #endif
void* cellDmaLargeGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) 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) #if defined (__SPU__) || defined (USE_LIBSPE2)
cellDmaLargeGet(ls,ea,size,tag,tid,rid); cellDmaLargeGet(ls,ea,size,tag,tid,rid);
return ls; return ls;
#else #else
return (void*)(uint32_t)ea; return (void*)(uint32_t)ea;
#endif #endif
} }
void* cellDmaSmallGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) 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) #if defined (__SPU__) || defined (USE_LIBSPE2)
mfc_get(ls,ea,size,tag,0,0); mfc_get(ls,ea,size,tag,0,0);
return ls; return ls;
#else #else
return (void*)(uint32_t)ea; return (void*)(uint32_t)ea;
#endif #endif
} }
void* cellDmaGetReadOnly(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) 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) #if defined (__SPU__) || defined (USE_LIBSPE2)
cellDmaGet(ls,ea,size,tag,tid,rid); cellDmaGet(ls,ea,size,tag,tid,rid);
return ls; return ls;
#else #else
return (void*)(uint32_t)ea; return (void*)(uint32_t)ea;
#endif #endif
} }
///this unalignedDma should not be frequently used, only for small data. It handles alignment and performs check on size (<16 bytes) ///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) int stallingUnalignedDmaSmallGet(void *ls, uint64_t ea, uint32_t size)
{ {
btAssert(size<32); btAssert(size<32);
ATTRIBUTE_ALIGNED16(char tmpBuffer[32]); ATTRIBUTE_ALIGNED16(char tmpBuffer[32]);
char* mainMem = (char*)ea; char* mainMem = (char*)ea;
char* localStore = (char*)ls; char* localStore = (char*)ls;
uint32_t i; uint32_t i;
///make sure last 4 bits are the same, for cellDmaSmallGet ///make sure last 4 bits are the same, for cellDmaSmallGet
uint32_t last4BitsOffset = ea & 0x0f; uint32_t last4BitsOffset = ea & 0x0f;
char* tmpTarget = tmpBuffer + last4BitsOffset; char* tmpTarget = tmpBuffer + last4BitsOffset;
#if defined (__SPU__) || defined (USE_LIBSPE2) #if defined (__SPU__) || defined (USE_LIBSPE2)
int remainingSize = size; int remainingSize = size;
//#define FORCE_cellDmaUnalignedGet 1 //#define FORCE_cellDmaUnalignedGet 1
#ifdef FORCE_cellDmaUnalignedGet #ifdef FORCE_cellDmaUnalignedGet
cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0); cellDmaUnalignedGet(tmpTarget,ea,size,DMA_TAG(1),0,0);
#else #else
char* remainingTmpTarget = tmpTarget; char* remainingTmpTarget = tmpTarget;
uint64_t remainingEa = ea; uint64_t remainingEa = ea;
while (remainingSize) while (remainingSize)
{ {
switch (remainingSize) switch (remainingSize)
{ {
case 1: case 1:
case 2: case 2:
case 4: case 4:
case 8: case 8:
case 16: case 16:
{ {
mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0); mfc_get(remainingTmpTarget,remainingEa,remainingSize,DMA_TAG(1),0,0);
remainingSize=0; remainingSize=0;
break; break;
} }
default: default:
{ {
//spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize); //spu_printf("unaligned DMA with non-natural size:%d\n",remainingSize);
int actualSize = 0; int actualSize = 0;
if (remainingSize > 16) if (remainingSize > 16)
actualSize = 16; actualSize = 16;
else else
if (remainingSize >8) if (remainingSize >8)
actualSize=8; actualSize=8;
else else
if (remainingSize >4) if (remainingSize >4)
actualSize=4; actualSize=4;
else else
if (remainingSize >2) if (remainingSize >2)
actualSize=2; actualSize=2;
mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0); mfc_get(remainingTmpTarget,remainingEa,actualSize,DMA_TAG(1),0,0);
remainingSize-=actualSize; remainingSize-=actualSize;
remainingTmpTarget+=actualSize; remainingTmpTarget+=actualSize;
remainingEa += actualSize; remainingEa += actualSize;
} }
} }
} }
#endif//FORCE_cellDmaUnalignedGet #endif//FORCE_cellDmaUnalignedGet
#else #else
//copy into final destination //copy into final destination
#ifdef USE_MEMCPY #ifdef USE_MEMCPY
memcpy(tmpTarget,mainMem,size); memcpy(tmpTarget,mainMem,size);
#else #else
for ( i=0;i<size;i++) for ( i=0;i<size;i++)
{ {
tmpTarget[i] = mainMem[i]; tmpTarget[i] = mainMem[i];
} }
#endif //USE_MEMCPY #endif //USE_MEMCPY
#endif #endif
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
//this is slowish, perhaps memcpy on SPU is smarter? //this is slowish, perhaps memcpy on SPU is smarter?
for (i=0; btLikely( i<size );i++) for (i=0; btLikely( i<size );i++)
{ {
localStore[i] = tmpTarget[i]; localStore[i] = tmpTarget[i];
} }
return 0; return 0;
} }
#if defined (__SPU__) || defined (USE_LIBSPE2) #if defined (__SPU__) || defined (USE_LIBSPE2)
#else #else
int cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) int cellDmaLargeGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
{ {
char* mainMem = (char*)ea; char* mainMem = (char*)ea;
char* localStore = (char*)ls; char* localStore = (char*)ls;
#ifdef USE_MEMCPY #ifdef USE_MEMCPY
memcpy(localStore,mainMem,size); memcpy(localStore,mainMem,size);
#else #else
for (uint32_t i=0;i<size;i++) for (uint32_t i=0;i<size;i++)
{ {
localStore[i] = mainMem[i]; localStore[i] = mainMem[i];
} }
#endif #endif
return 0; return 0;
} }
int cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) int cellDmaGet(void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
{ {
char* mainMem = (char*)ea; char* mainMem = (char*)ea;
char* localStore = (char*)ls; char* localStore = (char*)ls;
#ifdef USE_MEMCPY #ifdef USE_MEMCPY
memcpy(localStore,mainMem,size); memcpy(localStore,mainMem,size);
#else #else
for (uint32_t i=0;i<size;i++) for (uint32_t i=0;i<size;i++)
{ {
localStore[i] = mainMem[i]; localStore[i] = mainMem[i];
} }
#endif //#ifdef USE_MEMCPY #endif //#ifdef USE_MEMCPY
return 0; return 0;
} }
int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid) int cellDmaLargePut(const void *ls, uint64_t ea, uint32_t size, uint32_t tag, uint32_t tid, uint32_t rid)
{ {
char* mainMem = (char*)ea; char* mainMem = (char*)ea;
const char* localStore = (const char*)ls; const char* localStore = (const char*)ls;
#ifdef USE_MEMCPY #ifdef USE_MEMCPY
memcpy(mainMem,localStore,size); memcpy(mainMem,localStore,size);
#else #else
for (uint32_t i=0;i<size;i++) for (uint32_t i=0;i<size;i++)
{ {
mainMem[i] = localStore[i]; mainMem[i] = localStore[i];
} }
#endif //#ifdef USE_MEMCPY #endif //#ifdef USE_MEMCPY
return 0; return 0;
} }
void cellDmaWaitTagStatusAll(int ignore) void cellDmaWaitTagStatusAll(int ignore)
{ {
} }
#endif #endif

138
src/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h
View File

@@ -1,69 +1,69 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_GATHERING_COLLISION__DISPATCHER_H #ifndef SPU_GATHERING_COLLISION__DISPATCHER_H
#define SPU_GATHERING_COLLISION__DISPATCHER_H #define SPU_GATHERING_COLLISION__DISPATCHER_H
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
///Tuning value to optimized SPU utilization ///Tuning value to optimized SPU utilization
///Too small value means Task overhead is large compared to computation (too fine granularity) ///Too small value means Task overhead is large compared to computation (too fine granularity)
///Too big value might render some SPUs are idle, while a few other SPUs are doing all work. ///Too big value might render some SPUs are idle, while a few other SPUs are doing all work.
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8 //#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16 //#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
#define SPU_BATCHSIZE_BROADPHASE_PAIRS 64 #define SPU_BATCHSIZE_BROADPHASE_PAIRS 64
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128 //#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256 //#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024 //#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024
class SpuCollisionTaskProcess; class SpuCollisionTaskProcess;
///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection ///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection
///Time of Impact, Closest Points and Penetration Depth. ///Time of Impact, Closest Points and Penetration Depth.
class SpuGatheringCollisionDispatcher : public btCollisionDispatcher class SpuGatheringCollisionDispatcher : public btCollisionDispatcher
{ {
SpuCollisionTaskProcess* m_spuCollisionTaskProcess; SpuCollisionTaskProcess* m_spuCollisionTaskProcess;
protected: protected:
class btThreadSupportInterface* m_threadInterface; class btThreadSupportInterface* m_threadInterface;
unsigned int m_maxNumOutstandingTasks; unsigned int m_maxNumOutstandingTasks;
public: public:
//can be used by SPU collision algorithms //can be used by SPU collision algorithms
SpuCollisionTaskProcess* getSpuCollisionTaskProcess() SpuCollisionTaskProcess* getSpuCollisionTaskProcess()
{ {
return m_spuCollisionTaskProcess; return m_spuCollisionTaskProcess;
} }
SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration); SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
virtual ~SpuGatheringCollisionDispatcher(); virtual ~SpuGatheringCollisionDispatcher();
bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1); bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ; virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
}; };
#endif //SPU_GATHERING_COLLISION__DISPATCHER_H #endif //SPU_GATHERING_COLLISION__DISPATCHER_H

2
src/BulletMultiThreaded/SpuIntegrationTask/readme.txt
View File

@@ -1 +1 @@
Empty placeholder for future Libspe2 SPU task Empty placeholder for future Libspe2 SPU task

328
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/Box.h
View File

@@ -1,164 +1,164 @@
/* /*
Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
All rights reserved. All rights reserved.
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef __BOX_H__ #ifndef __BOX_H__
#define __BOX_H__ #define __BOX_H__
#ifndef PE_REF #ifndef PE_REF
#define PE_REF(a) a& #define PE_REF(a) a&
#endif #endif
#include <math.h> #include <math.h>
#include <vectormath_aos.h> #include <vectormath_aos.h>
using namespace Vectormath::Aos; using namespace Vectormath::Aos;
enum FeatureType { F, E, V }; enum FeatureType { F, E, V };
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
// Box // Box
//---------------------------------------------------------------------------- //----------------------------------------------------------------------------
///The Box is an internal class used by the boxBoxDistance calculation. ///The Box is an internal class used by the boxBoxDistance calculation.
class Box class Box
{ {
public: public:
Vector3 half; Vector3 half;
inline Box() inline Box()
{} {}
inline Box(PE_REF(Vector3) half_); inline Box(PE_REF(Vector3) half_);
inline Box(float hx, float hy, float hz); inline Box(float hx, float hy, float hz);
inline void Set(PE_REF(Vector3) half_); inline void Set(PE_REF(Vector3) half_);
inline void Set(float hx, float hy, float hz); inline void Set(float hx, float hy, float hz);
inline Vector3 GetAABB(const Matrix3& rotation) const; inline Vector3 GetAABB(const Matrix3& rotation) const;
}; };
inline inline
Box::Box(PE_REF(Vector3) half_) Box::Box(PE_REF(Vector3) half_)
{ {
Set(half_); Set(half_);
} }
inline inline
Box::Box(float hx, float hy, float hz) Box::Box(float hx, float hy, float hz)
{ {
Set(hx, hy, hz); Set(hx, hy, hz);
} }
inline inline
void void
Box::Set(PE_REF(Vector3) half_) Box::Set(PE_REF(Vector3) half_)
{ {
half = half_; half = half_;
} }
inline inline
void void
Box::Set(float hx, float hy, float hz) Box::Set(float hx, float hy, float hz)
{ {
half = Vector3(hx, hy, hz); half = Vector3(hx, hy, hz);
} }
inline inline
Vector3 Vector3
Box::GetAABB(const Matrix3& rotation) const Box::GetAABB(const Matrix3& rotation) const
{ {
return absPerElem(rotation) * half; return absPerElem(rotation) * half;
} }
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
// BoxPoint // BoxPoint
//------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------
///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation. ///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation.
class BoxPoint class BoxPoint
{ {
public: public:
BoxPoint() : localPoint(0.0f) {} BoxPoint() : localPoint(0.0f) {}
Point3 localPoint; Point3 localPoint;
FeatureType featureType; FeatureType featureType;
int featureIdx; int featureIdx;
inline void setVertexFeature(int plusX, int plusY, int plusZ); inline void setVertexFeature(int plusX, int plusY, int plusZ);
inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1); inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1);
inline void setFaceFeature(int dim, int plus); inline void setFaceFeature(int dim, int plus);
inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const; inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const;
inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const; inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const;
inline void getFaceFeature(int & dim, int & plus) const; inline void getFaceFeature(int & dim, int & plus) const;
}; };
inline inline
void void
BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ) BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ)
{ {
featureType = V; featureType = V;
featureIdx = plusX << 2 | plusY << 1 | plusZ; featureIdx = plusX << 2 | plusY << 1 | plusZ;
} }
inline inline
void void
BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1) BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1)
{ {
featureType = E; featureType = E;
if (dim0 > dim1) { if (dim0 > dim1) {
featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0; featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0;
} else { } else {
featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1; featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1;
} }
} }
inline inline
void void
BoxPoint::setFaceFeature(int dim, int plus) BoxPoint::setFaceFeature(int dim, int plus)
{ {
featureType = F; featureType = F;
featureIdx = plus << 2 | dim; featureIdx = plus << 2 | dim;
} }
inline inline
void void
BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const
{ {
plusX = featureIdx >> 2; plusX = featureIdx >> 2;
plusY = featureIdx >> 1 & 1; plusY = featureIdx >> 1 & 1;
plusZ = featureIdx & 1; plusZ = featureIdx & 1;
} }
inline inline
void void
BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const
{ {
plus0 = featureIdx >> 5; plus0 = featureIdx >> 5;
dim0 = featureIdx >> 3 & 3; dim0 = featureIdx >> 3 & 3;
plus1 = featureIdx >> 2 & 1; plus1 = featureIdx >> 2 & 1;
dim1 = featureIdx & 3; dim1 = featureIdx & 3;
} }
inline inline
void void
BoxPoint::getFaceFeature(int & dim, int & plus) const BoxPoint::getFaceFeature(int & dim, int & plus) const
{ {
plus = featureIdx >> 2; plus = featureIdx >> 2;
dim = featureIdx & 3; dim = featureIdx & 3;
} }
#endif /* __BOX_H__ */ #endif /* __BOX_H__ */

1084
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.cpp
View File

File diff suppressed because it is too large Load Diff

250
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h
View File

@@ -1,125 +1,125 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef __SPU_COLLISION_SHAPES_H #ifndef __SPU_COLLISION_SHAPES_H
#define __SPU_COLLISION_SHAPES_H #define __SPU_COLLISION_SHAPES_H
#include "../SpuDoubleBuffer.h" #include "../SpuDoubleBuffer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h" #include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" #include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" #include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h"
#define MAX_NUM_SPU_CONVEX_POINTS 128 #define MAX_NUM_SPU_CONVEX_POINTS 128
struct SpuConvexPolyhedronVertexData struct SpuConvexPolyhedronVertexData
{ {
void* gSpuConvexShapePtr; void* gSpuConvexShapePtr;
btVector3* gConvexPoints; btVector3* gConvexPoints;
int gNumConvexPoints; int gNumConvexPoints;
ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]); ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
}; };
#define MAX_SHAPE_SIZE 256 #define MAX_SHAPE_SIZE 256
struct CollisionShape_LocalStoreMemory struct CollisionShape_LocalStoreMemory
{ {
ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]); ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
}; };
struct CompoundShape_LocalStoreMemory struct CompoundShape_LocalStoreMemory
{ {
// Compound data // Compound data
#define MAX_SPU_COMPOUND_SUBSHAPES 16 #define MAX_SPU_COMPOUND_SUBSHAPES 16
ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]); ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]); ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
}; };
struct bvhMeshShape_LocalStoreMemory struct bvhMeshShape_LocalStoreMemory
{ {
//ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh); //ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh);
ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]); ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
btOptimizedBvh* getOptimizedBvh() btOptimizedBvh* getOptimizedBvh()
{ {
return (btOptimizedBvh*) gOptimizedBvh; return (btOptimizedBvh*) gOptimizedBvh;
} }
ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage); ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage);
btTriangleIndexVertexArray* gTriangleMeshInterfacePtr; 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 ///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); ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh);
#define MAX_SPU_SUBTREE_HEADERS 32 #define MAX_SPU_SUBTREE_HEADERS 32
//1024 //1024
ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]); ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]); 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) 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 computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform);
void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape); void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag); 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 dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag); void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag);
int getShapeTypeSize(int shapeType); int getShapeTypeSize(int shapeType);
void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU); void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType); void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag); void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
#define USE_BRANCHFREE_TEST 1 #define USE_BRANCHFREE_TEST 1
#ifdef USE_BRANCHFREE_TEST #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) 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__) #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 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 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0};
vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax); vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax);
return spu_extract(spu_gather(isGt),0)==0; return spu_extract(spu_gather(isGt),0)==0;
#else #else
return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0); 1, 0);
#endif #endif
} }
#else #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) 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; unsigned int overlap = 1;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap; overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap; overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap; overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
return overlap; return overlap;
} }
#endif #endif
void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex); void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
#endif #endif

462
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp
View File

@@ -1,231 +1,231 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuContactResult.h" #include "SpuContactResult.h"
//#define DEBUG_SPU_COLLISION_DETECTION 1 //#define DEBUG_SPU_COLLISION_DETECTION 1
SpuContactResult::SpuContactResult() SpuContactResult::SpuContactResult()
{ {
m_manifoldAddress = 0; m_manifoldAddress = 0;
m_spuManifold = NULL; m_spuManifold = NULL;
m_RequiresWriteBack = false; m_RequiresWriteBack = false;
} }
SpuContactResult::~SpuContactResult() SpuContactResult::~SpuContactResult()
{ {
g_manifoldDmaExport.swapBuffers(); g_manifoldDmaExport.swapBuffers();
} }
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1) inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1)
{ {
btScalar friction = friction0*friction1; btScalar friction = friction0*friction1;
const btScalar MAX_FRICTION = btScalar(10.); const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION) if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION; friction = -MAX_FRICTION;
if (friction > MAX_FRICTION) if (friction > MAX_FRICTION)
friction = MAX_FRICTION; friction = MAX_FRICTION;
return friction; return friction;
} }
inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1) inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
{ {
return restitution0*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) 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); //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
m_rootWorldTransform0 = worldTrans0; m_rootWorldTransform0 = worldTrans0;
m_rootWorldTransform1 = worldTrans1; m_rootWorldTransform1 = worldTrans1;
m_manifoldAddress = manifoldAddress; m_manifoldAddress = manifoldAddress;
m_spuManifold = spuManifold; m_spuManifold = spuManifold;
m_combinedFriction = calculateCombinedFriction(friction0,friction1); m_combinedFriction = calculateCombinedFriction(friction0,friction1);
m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1); m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
m_isSwapped = isSwapped; m_isSwapped = isSwapped;
} }
void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1) void SpuContactResult::setShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{ {
} }
///return true if it requires a dma transfer back ///return true if it requires a dma transfer back
bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld, bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
const btVector3& pointInWorld, const btVector3& pointInWorld,
float depth, float depth,
btPersistentManifold* manifoldPtr, btPersistentManifold* manifoldPtr,
btTransform& transA, btTransform& transA,
btTransform& transB, btTransform& transB,
btScalar combinedFriction, btScalar combinedFriction,
btScalar combinedRestitution, btScalar combinedRestitution,
bool isSwapped) bool isSwapped)
{ {
float contactTreshold = manifoldPtr->getContactBreakingThreshold(); float contactTreshold = manifoldPtr->getContactBreakingThreshold();
//spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr); //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
#ifdef DEBUG_SPU_COLLISION_DETECTION #ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: contactTreshold %f\n",contactTreshold); spu_printf("SPU: contactTreshold %f\n",contactTreshold);
#endif //DEBUG_SPU_COLLISION_DETECTION #endif //DEBUG_SPU_COLLISION_DETECTION
if (depth > manifoldPtr->getContactBreakingThreshold()) if (depth > manifoldPtr->getContactBreakingThreshold())
return false; return false;
//provide inverses or just calculate? //provide inverses or just calculate?
btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform; btTransform transAInv = transA.inverse();//m_body0->m_cachedInvertedWorldTransform;
btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform; btTransform transBInv= transB.inverse();//m_body1->m_cachedInvertedWorldTransform;
btVector3 pointA; btVector3 pointA;
btVector3 localA; btVector3 localA;
btVector3 localB; btVector3 localB;
btVector3 normal; btVector3 normal;
if (isSwapped) if (isSwapped)
{ {
normal = normalOnBInWorld * -1; normal = normalOnBInWorld * -1;
pointA = pointInWorld + normal * depth; pointA = pointInWorld + normal * depth;
localA = transAInv(pointA ); localA = transAInv(pointA );
localB = transBInv(pointInWorld); localB = transBInv(pointInWorld);
/*localA = transBInv(pointA ); /*localA = transBInv(pointA );
localB = transAInv(pointInWorld);*/ localB = transAInv(pointInWorld);*/
} }
else else
{ {
normal = normalOnBInWorld; normal = normalOnBInWorld;
pointA = pointInWorld + normal * depth; pointA = pointInWorld + normal * depth;
localA = transAInv(pointA ); localA = transAInv(pointA );
localB = transBInv(pointInWorld); localB = transBInv(pointInWorld);
} }
btManifoldPoint newPt(localA,localB,normal,depth); btManifoldPoint newPt(localA,localB,normal,depth);
int insertIndex = manifoldPtr->getCacheEntry(newPt); int insertIndex = manifoldPtr->getCacheEntry(newPt);
if (insertIndex >= 0) if (insertIndex >= 0)
{ {
// manifoldPtr->replaceContactPoint(newPt,insertIndex); // manifoldPtr->replaceContactPoint(newPt,insertIndex);
// return true; // return true;
#ifdef DEBUG_SPU_COLLISION_DETECTION #ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: same contact detected, nothing done\n"); spu_printf("SPU: same contact detected, nothing done\n");
#endif //DEBUG_SPU_COLLISION_DETECTION #endif //DEBUG_SPU_COLLISION_DETECTION
// This is not needed, just use the old info! saves a DMA transfer as well // This is not needed, just use the old info! saves a DMA transfer as well
} else } else
{ {
newPt.m_combinedFriction = combinedFriction; newPt.m_combinedFriction = combinedFriction;
newPt.m_combinedRestitution = combinedRestitution; newPt.m_combinedRestitution = combinedRestitution;
/* /*
///@todo: SPU callbacks, either immediate (local on the SPU), or deferred ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
//User can override friction and/or restitution //User can override friction and/or restitution
if (gContactAddedCallback && if (gContactAddedCallback &&
//and if either of the two bodies requires custom material //and if either of the two bodies requires custom material
((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) || ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
(m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback))) (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
{ {
//experimental feature info, for per-triangle material etc. //experimental feature info, for per-triangle material etc.
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1); (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
} }
*/ */
manifoldPtr->addManifoldPoint(newPt); manifoldPtr->addManifoldPoint(newPt);
return true; return true;
} }
return false; return false;
} }
void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold) 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 ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place
///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly' ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly'
#if defined (__SPU__) || defined (USE_LIBSPE2) #if defined (__SPU__) || defined (USE_LIBSPE2)
memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold)); memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
g_manifoldDmaExport.swapBuffers(); g_manifoldDmaExport.swapBuffers();
ppu_address_t mmAddr = (ppu_address_t)mmManifold; ppu_address_t mmAddr = (ppu_address_t)mmManifold;
g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9)); 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? // 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 //no, the swapBuffers does the wait
#endif #endif
} }
void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)
{ {
//spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth); //spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
#ifdef DEBUG_SPU_COLLISION_DETECTION #ifdef DEBUG_SPU_COLLISION_DETECTION
// int sman = sizeof(rage::phManifold); // int sman = sizeof(rage::phManifold);
// spu_printf("sizeof_manifold = %i\n",sman); // spu_printf("sizeof_manifold = %i\n",sman);
#endif //DEBUG_SPU_COLLISION_DETECTION #endif //DEBUG_SPU_COLLISION_DETECTION
btPersistentManifold* localManifold = m_spuManifold; btPersistentManifold* localManifold = m_spuManifold;
btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ()); btVector3 normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ()); btVector3 pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
//process the contact point //process the contact point
const bool retVal = ManifoldResultAddContactPoint(normalB, const bool retVal = ManifoldResultAddContactPoint(normalB,
pointWrld, pointWrld,
depth, depth,
localManifold, localManifold,
m_rootWorldTransform0, m_rootWorldTransform0,
m_rootWorldTransform1, m_rootWorldTransform1,
m_combinedFriction, m_combinedFriction,
m_combinedRestitution, m_combinedRestitution,
m_isSwapped); m_isSwapped);
m_RequiresWriteBack = m_RequiresWriteBack || retVal; m_RequiresWriteBack = m_RequiresWriteBack || retVal;
} }
void SpuContactResult::flush() void SpuContactResult::flush()
{ {
if (m_spuManifold && m_spuManifold->getNumContacts()) if (m_spuManifold && m_spuManifold->getNumContacts())
{ {
m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1); m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
m_RequiresWriteBack = true; m_RequiresWriteBack = true;
} }
if (m_RequiresWriteBack) if (m_RequiresWriteBack)
{ {
#ifdef DEBUG_SPU_COLLISION_DETECTION #ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n"); spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts()); spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
spu_printf("Manifold address: %llu\n", m_manifoldAddress); spu_printf("Manifold address: %llu\n", m_manifoldAddress);
#endif //DEBUG_SPU_COLLISION_DETECTION #endif //DEBUG_SPU_COLLISION_DETECTION
// spu_printf("writeDoubleBufferedManifold\n"); // spu_printf("writeDoubleBufferedManifold\n");
writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress); writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
#ifdef DEBUG_SPU_COLLISION_DETECTION #ifdef DEBUG_SPU_COLLISION_DETECTION
spu_printf("SPU: Finished (Put) DMA\n"); spu_printf("SPU: Finished (Put) DMA\n");
#endif //DEBUG_SPU_COLLISION_DETECTION #endif //DEBUG_SPU_COLLISION_DETECTION
} }
m_spuManifold = NULL; m_spuManifold = NULL;
m_RequiresWriteBack = false; m_RequiresWriteBack = false;
} }

2320
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.cpp
View File

File diff suppressed because it is too large Load Diff

1702
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGjkEpa2.cpp
View File

File diff suppressed because it is too large Load Diff

2310
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.cpp
View File

File diff suppressed because it is too large Load Diff

132
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/boxBoxDistance.h
View File

@@ -1,66 +1,66 @@
/* /*
Copyright (C) 2006, 2008 Sony Computer Entertainment Inc. Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
All rights reserved. All rights reserved.
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef __BOXBOXDISTANCE_H__ #ifndef __BOXBOXDISTANCE_H__
#define __BOXBOXDISTANCE_H__ #define __BOXBOXDISTANCE_H__
#include "Box.h" #include "Box.h"
using namespace Vectormath::Aos; using namespace Vectormath::Aos;
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
// boxBoxDistance: // boxBoxDistance:
// //
// description: // description:
// this computes info that can be used for the collision response of two boxes. when the boxes // 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 // 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 // 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. // 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 // 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). // overlap (it's being approximated here as one of the 15 "separating axis" directions).
// //
// returns: // returns:
// positive or negative distance between two boxes. // positive or negative distance between two boxes.
// //
// args: // args:
// Vector3& normal: set to a unit contact normal pointing from box A to box B. // Vector3& normal: set to a unit contact normal pointing from box A to box B.
// //
// BoxPoint& boxPointA, BoxPoint& boxPointB: // BoxPoint& boxPointA, BoxPoint& boxPointB:
// set to a closest point or point of penetration on each box. // set to a closest point or point of penetration on each box.
// //
// Box boxA, Box boxB: // Box boxA, Box boxB:
// boxes, represented as 3 half-widths // boxes, represented as 3 half-widths
// //
// const Transform3& transformA, const Transform3& transformB: // const Transform3& transformA, const Transform3& transformB:
// box transformations, in world coordinates // box transformations, in world coordinates
// //
// float distanceThreshold: // float distanceThreshold:
// the algorithm will exit early if it finds that the boxes are more distant than this // 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 // 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 // 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. // default, this is set to MAX_FLOAT so it will have no effect.
// //
//--------------------------------------------------------------------------- //---------------------------------------------------------------------------
float float
boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB, boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB, PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB,
const Transform3 & transformB, const Transform3 & transformB,
float distanceThreshold = FLT_MAX ); float distanceThreshold = FLT_MAX );
#endif /* __BOXBOXDISTANCE_H__ */ #endif /* __BOXBOXDISTANCE_H__ */

2
src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/readme.txt
View File

@@ -1 +1 @@
Empty placeholder for future Libspe2 SPU task Empty placeholder for future Libspe2 SPU task

1572
src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.cpp
View File

File diff suppressed because it is too large Load Diff

100
src/BulletMultiThreaded/SpuRaycastTask/SpuRaycastTask.h
View File

@@ -1,50 +1,50 @@
#ifndef __SPU_RAYCAST_TASK_H #ifndef __SPU_RAYCAST_TASK_H
#define __SPU_RAYCAST_TASK_H #define __SPU_RAYCAST_TASK_H
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "../PlatformDefinitions.h" #include "../PlatformDefinitions.h"
ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData ATTRIBUTE_ALIGNED16(struct) RaycastGatheredObjectData
{ {
ppu_address_t m_collisionShape; ppu_address_t m_collisionShape;
void* m_spuCollisionShape; void* m_spuCollisionShape;
btVector3 m_primitiveDimensions; btVector3 m_primitiveDimensions;
int m_shapeType; int m_shapeType;
float m_collisionMargin; float m_collisionMargin;
btTransform m_worldTransform; btTransform m_worldTransform;
}; };
ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnitOut
{ {
btVector3 hitNormal; /* out */ btVector3 hitNormal; /* out */
btScalar hitFraction; /* out */ btScalar hitFraction; /* out */
btCollisionWorld::LocalShapeInfo shapeInfo; /* out */ btCollisionWorld::LocalShapeInfo shapeInfo; /* out */
}; };
/* Perform a raycast on collision object */ /* Perform a raycast on collision object */
ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit ATTRIBUTE_ALIGNED16(struct) SpuRaycastTaskWorkUnit
{ {
btVector3 rayFrom; /* in */ btVector3 rayFrom; /* in */
btVector3 rayTo; /* in */ btVector3 rayTo; /* in */
SpuRaycastTaskWorkUnitOut* output; /* out */ SpuRaycastTaskWorkUnitOut* output; /* out */
}; };
#define SPU_RAYCAST_WORK_UNITS_PER_TASK 16 #define SPU_RAYCAST_WORK_UNITS_PER_TASK 16
ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc ATTRIBUTE_ALIGNED128(struct) SpuRaycastTaskDesc
{ {
SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK]; SpuRaycastTaskWorkUnit workUnits[SPU_RAYCAST_WORK_UNITS_PER_TASK];
unsigned int numWorkUnits; unsigned int numWorkUnits;
void* spuCollisionObjectsWrappers; void* spuCollisionObjectsWrappers;
unsigned int numSpuCollisionObjectWrappers; unsigned int numSpuCollisionObjectWrappers;
int taskId; int taskId;
}; };
void processRaycastTask (void* userPtr, void* lsMemory); void processRaycastTask (void* userPtr, void* lsMemory);
void* createRaycastLocalStoreMemory (); void* createRaycastLocalStoreMemory ();
#endif #endif

304
src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.cpp
View File

@@ -1,152 +1,152 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuSubSimplexConvexCast.h" #include "SpuSubSimplexConvexCast.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, SpuSubsimplexRayCast::SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
SpuVoronoiSimplexSolver* simplexSolver) SpuVoronoiSimplexSolver* simplexSolver)
:m_simplexSolver(simplexSolver), m_shapeB(shapeB), m_convexDataB(convexDataB), m_shapeTypeB(shapeTypeB), m_marginB(marginB) :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. ///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 ///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
#ifdef BT_USE_DOUBLE_PRECISION #ifdef BT_USE_DOUBLE_PRECISION
#define MAX_ITERATIONS 64 #define MAX_ITERATIONS 64
#else #else
#define MAX_ITERATIONS 32 #define MAX_ITERATIONS 32
#endif #endif
/* Returns the support point of the minkowski sum: /* Returns the support point of the minkowski sum:
* MSUM(Pellet, ConvexShape) * MSUM(Pellet, ConvexShape)
* *
*/ */
void supportPoints (const btTransform& xformRay, void supportPoints (const btTransform& xformRay,
const btTransform& xformB, const btTransform& xformB,
const int shapeType, const int shapeType,
const void* shape, const void* shape,
SpuConvexPolyhedronVertexData* convexVertexData, SpuConvexPolyhedronVertexData* convexVertexData,
const btScalar marginB, const btScalar marginB,
const btVector3& seperatingAxis, const btVector3& seperatingAxis,
btVector3& w, btVector3& w,
btVector3& supVertexRay, btVector3& supVertexRay,
btVector3& supVertexB) btVector3& supVertexB)
{ {
btVector3 saUnit = seperatingAxis; btVector3 saUnit = seperatingAxis;
saUnit.normalize(); saUnit.normalize();
btVector3 SupportPellet = xformRay(0.0001 * -saUnit); btVector3 SupportPellet = xformRay(0.0001 * -saUnit);
btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis(); btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis();
btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData)); btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData));
SupportShape += saUnit * marginB; SupportShape += saUnit * marginB;
w = SupportPellet - SupportShape; w = SupportPellet - SupportShape;
supVertexRay = SupportPellet; supVertexRay = SupportPellet;
supVertexB = SupportShape; supVertexB = SupportShape;
} }
bool SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay, bool SpuSubsimplexRayCast::calcTimeOfImpact(const btTransform& fromRay,
const btTransform& toRay, const btTransform& toRay,
const btTransform& fromB, const btTransform& fromB,
const btTransform& toB, const btTransform& toB,
SpuCastResult& result) SpuCastResult& result)
{ {
m_simplexSolver->reset(); m_simplexSolver->reset();
btVector3 linVelRay, linVelB; btVector3 linVelRay, linVelB;
linVelRay = toRay.getOrigin() - fromRay.getOrigin(); linVelRay = toRay.getOrigin() - fromRay.getOrigin();
linVelB = toB.getOrigin() - fromB.getOrigin (); linVelB = toB.getOrigin() - fromB.getOrigin ();
btScalar lambda = btScalar(0.); btScalar lambda = btScalar(0.);
btTransform interpolatedTransRay = fromRay; btTransform interpolatedTransRay = fromRay;
btTransform interpolatedTransB = fromB; btTransform interpolatedTransB = fromB;
btVector3 r = (linVelRay-linVelB); btVector3 r = (linVelRay-linVelB);
btVector3 supVertexRay; btVector3 supVertexRay;
btVector3 supVertexB; btVector3 supVertexB;
btVector3 v; btVector3 v;
supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB); supportPoints (fromRay, fromB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, r, v, supVertexRay, supVertexB);
btVector3 n; btVector3 n;
n.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
bool hasResult = false; bool hasResult = false;
btVector3 c; btVector3 c;
int maxIter = MAX_ITERATIONS; int maxIter = MAX_ITERATIONS;
btScalar lastLambda = lambda; btScalar lastLambda = lambda;
btScalar dist2 = v.length2(); btScalar dist2 = v.length2();
#ifdef BT_USE_DOUBLE_PRECISION #ifdef BT_USE_DOUBLE_PRECISION
btScalar epsilon = btScalar(0.0001); btScalar epsilon = btScalar(0.0001);
#else #else
btScalar epsilon = btScalar(0.0001); btScalar epsilon = btScalar(0.0001);
#endif //BT_USE_DOUBLE_PRECISION #endif //BT_USE_DOUBLE_PRECISION
btVector3 w,p; btVector3 w,p;
btScalar VdotR; btScalar VdotR;
while ( (dist2 > epsilon) && maxIter--) while ( (dist2 > epsilon) && maxIter--)
{ {
supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB); supportPoints (interpolatedTransRay, interpolatedTransB, m_shapeTypeB, m_shapeB, m_convexDataB, m_marginB, v, w, supVertexRay, supVertexB);
btScalar VdotW = v.dot(w); btScalar VdotW = v.dot(w);
if (lambda > btScalar(1.0)) if (lambda > btScalar(1.0))
{ {
return false; return false;
} }
if ( VdotW > btScalar(0.)) if ( VdotW > btScalar(0.))
{ {
VdotR = v.dot(r); VdotR = v.dot(r);
if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
return false; return false;
else else
{ {
lambda = lambda - VdotW / VdotR; lambda = lambda - VdotW / VdotR;
interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda); interpolatedTransRay.getOrigin().setInterpolate3(fromRay.getOrigin(), toRay.getOrigin(), lambda);
interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda); interpolatedTransB.getOrigin().setInterpolate3(fromB.getOrigin(), toB.getOrigin(), lambda);
lastLambda = lambda; lastLambda = lambda;
n = v; n = v;
hasResult = true; hasResult = true;
} }
} }
m_simplexSolver->addVertex(w, supVertexRay, supVertexB); m_simplexSolver->addVertex(w, supVertexRay, supVertexB);
if (m_simplexSolver->closest(v)) if (m_simplexSolver->closest(v))
{ {
dist2 = v.length2(); dist2 = v.length2();
hasResult = true; hasResult = true;
//printf("V=%f , %f, %f\n",v[0],v[1],v[2]); //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
//printf("DIST2=%f\n",dist2); //printf("DIST2=%f\n",dist2);
//printf("numverts = %i\n",m_simplexSolver->numVertices()); //printf("numverts = %i\n",m_simplexSolver->numVertices());
} else } else
{ {
dist2 = btScalar(0.); dist2 = btScalar(0.);
} }
} }
result.m_fraction = lambda; result.m_fraction = lambda;
result.m_normal = n; result.m_normal = n;
btVector3 hitRay, hitB; btVector3 hitRay, hitB;
m_simplexSolver->compute_points (hitRay, hitB); m_simplexSolver->compute_points (hitRay, hitB);
/* TODO: We could output hit point here (hitB) */ /* TODO: We could output hit point here (hitB) */
return true; return true;
} }

120
src/BulletMultiThreaded/SpuRaycastTask/SpuSubSimplexConvexCast.h
View File

@@ -1,60 +1,60 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_SUBSIMPLEX_RAY_CAST_H #ifndef SPU_SUBSIMPLEX_RAY_CAST_H
#define SPU_SUBSIMPLEX_RAY_CAST_H #define SPU_SUBSIMPLEX_RAY_CAST_H
#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h" #include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h"
#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h" #include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
#include "SpuRaycastTask.h" #include "SpuRaycastTask.h"
class btConvexShape; class btConvexShape;
struct SpuCastResult struct SpuCastResult
{ {
float m_fraction; float m_fraction;
btVector3 m_normal; btVector3 m_normal;
}; };
/// btSubsimplexConvexCast implements Gino van den Bergens' paper /// btSubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" ///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
/// GJK based Ray Cast, optimized version /// GJK based Ray Cast, optimized version
/// Objects should not start in overlap, otherwise results are not defined. /// Objects should not start in overlap, otherwise results are not defined.
class SpuSubsimplexRayCast class SpuSubsimplexRayCast
{ {
SpuVoronoiSimplexSolver* m_simplexSolver; SpuVoronoiSimplexSolver* m_simplexSolver;
void* m_shapeB; void* m_shapeB;
SpuConvexPolyhedronVertexData* m_convexDataB; SpuConvexPolyhedronVertexData* m_convexDataB;
int m_shapeTypeB; int m_shapeTypeB;
float m_marginB; float m_marginB;
public: public:
SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB, SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
SpuVoronoiSimplexSolver* simplexSolver); SpuVoronoiSimplexSolver* simplexSolver);
//virtual ~btSubsimplexConvexCast(); //virtual ~btSubsimplexConvexCast();
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. ///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. ///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, bool calcTimeOfImpact(const btTransform& fromRay,
const btTransform& toRay, const btTransform& toRay,
const btTransform& fromB, const btTransform& fromB,
const btTransform& toB, const btTransform& toB,
SpuCastResult& result); SpuCastResult& result);
}; };
#endif //SUBSIMPLEX_RAY_CAST_H #endif //SUBSIMPLEX_RAY_CAST_H

378
src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp
View File

@@ -1,189 +1,189 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuRaycastTaskProcess.h" #include "SpuRaycastTaskProcess.h"
SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks) SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks)
:m_threadInterface(threadInterface), :m_threadInterface(threadInterface),
m_maxNumOutstandingTasks(maxNumOutstandingTasks) m_maxNumOutstandingTasks(maxNumOutstandingTasks)
{ {
m_workUnitTaskBuffers = (unsigned char *)0; m_workUnitTaskBuffers = (unsigned char *)0;
m_taskBusy.resize(m_maxNumOutstandingTasks); m_taskBusy.resize(m_maxNumOutstandingTasks);
m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks); m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks);
for (int i = 0; i < m_maxNumOutstandingTasks; i++) for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{ {
m_taskBusy[i] = false; m_taskBusy[i] = false;
} }
m_numBusyTasks = 0; m_numBusyTasks = 0;
m_currentTask = 0; m_currentTask = 0;
m_currentWorkUnitInTask = 0; m_currentWorkUnitInTask = 0;
m_threadInterface->startSPU(); m_threadInterface->startSPU();
//printf("sizeof vec_float4: %d\n", sizeof(vec_float4)); //printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
//printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput)); //printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
} }
SpuRaycastTaskProcess::~SpuRaycastTaskProcess() SpuRaycastTaskProcess::~SpuRaycastTaskProcess()
{ {
if (m_workUnitTaskBuffers != 0) if (m_workUnitTaskBuffers != 0)
{ {
btAlignedFree(m_workUnitTaskBuffers); btAlignedFree(m_workUnitTaskBuffers);
m_workUnitTaskBuffers = 0; m_workUnitTaskBuffers = 0;
} }
m_threadInterface->stopSPU(); m_threadInterface->stopSPU();
} }
void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers) void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers)
{ {
m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers; m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers;
m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers; m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers;
for (int i = 0; i < m_maxNumOutstandingTasks; i++) for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{ {
m_taskBusy[i] = false; m_taskBusy[i] = false;
} }
m_numBusyTasks = 0; m_numBusyTasks = 0;
m_currentTask = 0; m_currentTask = 0;
m_currentWorkUnitInTask = 0; m_currentWorkUnitInTask = 0;
#ifdef DEBUG_SpuRaycastTaskProcess #ifdef DEBUG_SpuRaycastTaskProcess
m_initialized = true; m_initialized = true;
#endif #endif
} }
void SpuRaycastTaskProcess::issueTask2() void SpuRaycastTaskProcess::issueTask2()
{ {
m_taskBusy[m_currentTask] = true; m_taskBusy[m_currentTask] = true;
m_numBusyTasks++; m_numBusyTasks++;
SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask]; SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask];
taskDesc.taskId = m_currentTask; taskDesc.taskId = m_currentTask;
m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask); m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask);
//printf("send thread requested for task %d\n", 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 all tasks busy, wait for spu event to clear the task.
if (m_numBusyTasks >= m_maxNumOutstandingTasks) if (m_numBusyTasks >= m_maxNumOutstandingTasks)
{ {
unsigned int taskId; unsigned int taskId;
unsigned int outputSize; unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++) for (int i=0;i<m_maxNumOutstandingTasks;i++)
{ {
if (m_taskBusy[i]) if (m_taskBusy[i])
{ {
taskId = i; taskId = i;
break; break;
} }
} }
m_threadInterface->waitForResponse(&taskId, &outputSize); m_threadInterface->waitForResponse(&taskId, &outputSize);
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize); //printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
m_taskBusy[taskId] = false; m_taskBusy[taskId] = false;
m_numBusyTasks--; m_numBusyTasks--;
} else { } else {
//printf("Sent request, not enough busy tasks\n"); //printf("Sent request, not enough busy tasks\n");
} }
} }
void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit) void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit)
{ {
m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit; m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit;
m_currentWorkUnitInTask++; m_currentWorkUnitInTask++;
if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK) if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK)
{ {
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
//printf("Task buffer full, issuing\n"); //printf("Task buffer full, issuing\n");
issueTask2 (); issueTask2 ();
//printf("Returned from issueTask2()\n"); //printf("Returned from issueTask2()\n");
m_currentWorkUnitInTask = 0; m_currentWorkUnitInTask = 0;
// find new task buffer // find new task buffer
for (int i = 0; i < m_maxNumOutstandingTasks; i++) for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{ {
if (!m_taskBusy[i]) if (!m_taskBusy[i])
{ {
m_currentTask = i; m_currentTask = i;
//init the task data //init the task data
break; break;
} }
} }
//printf("next task = %d\n", m_currentTask); //printf("next task = %d\n", m_currentTask);
} }
} }
void void
SpuRaycastTaskProcess::flush2() SpuRaycastTaskProcess::flush2()
{ {
#ifdef DEBUG_SPU_TASK_SCHEDULING #ifdef DEBUG_SPU_TASK_SCHEDULING
printf("\nSpuRaycastTaskProcess::flush()\n"); printf("\nSpuRaycastTaskProcess::flush()\n");
#endif //DEBUG_SPU_TASK_SCHEDULING #endif //DEBUG_SPU_TASK_SCHEDULING
// if there's a partially filled task buffer, submit that task // if there's a partially filled task buffer, submit that task
//printf("Flushing... %d remaining\n", m_currentWorkUnitInTask); //printf("Flushing... %d remaining\n", m_currentWorkUnitInTask);
if (m_currentWorkUnitInTask > 0) if (m_currentWorkUnitInTask > 0)
{ {
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask; m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers; m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers; m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
issueTask2(); issueTask2();
m_currentWorkUnitInTask = 0; m_currentWorkUnitInTask = 0;
} }
// all tasks are issued, wait for all tasks to be complete // all tasks are issued, wait for all tasks to be complete
while(m_numBusyTasks > 0) while(m_numBusyTasks > 0)
{ {
// Consolidating SPU code // Consolidating SPU code
unsigned int taskId; unsigned int taskId;
unsigned int outputSize; unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++) for (int i=0;i<m_maxNumOutstandingTasks;i++)
{ {
if (m_taskBusy[i]) if (m_taskBusy[i])
{ {
taskId = i; taskId = i;
break; break;
} }
} }
//printf("Busy tasks... %d\n", m_numBusyTasks); //printf("Busy tasks... %d\n", m_numBusyTasks);
{ {
// SPURS support. // SPURS support.
m_threadInterface->waitForResponse(&taskId, &outputSize); m_threadInterface->waitForResponse(&taskId, &outputSize);
} }
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize); //printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
//postProcess(taskId, outputSize); //postProcess(taskId, outputSize);
m_taskBusy[taskId] = false; m_taskBusy[taskId] = false;
m_numBusyTasks--; m_numBusyTasks--;
} }
} }

428
src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.cpp
View File

@@ -1,214 +1,214 @@
/* /*
Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "SpuSampleTask.h" #include "SpuSampleTask.h"
#include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Dynamics/btRigidBody.h"
#include "../PlatformDefinitions.h" #include "../PlatformDefinitions.h"
#include "../SpuFakeDma.h" #include "../SpuFakeDma.h"
#include "LinearMath/btMinMax.h" #include "LinearMath/btMinMax.h"
#ifdef __SPU__ #ifdef __SPU__
#include <spu_printf.h> #include <spu_printf.h>
#else #else
#include <stdio.h> #include <stdio.h>
#define spu_printf printf #define spu_printf printf
#endif #endif
#define MAX_NUM_BODIES 8192 #define MAX_NUM_BODIES 8192
struct SampleTask_LocalStoreMemory struct SampleTask_LocalStoreMemory
{ {
ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]); ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]);
ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]); ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]);
}; };
//-- MAIN METHOD //-- MAIN METHOD
void processSampleTask(void* userPtr, void* lsMemory) void processSampleTask(void* userPtr, void* lsMemory)
{ {
// BT_PROFILE("processSampleTask"); // BT_PROFILE("processSampleTask");
SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory; SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory;
SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr; SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr;
SpuSampleTaskDesc& taskDesc = *taskDescPtr; SpuSampleTaskDesc& taskDesc = *taskDescPtr;
switch (taskDesc.m_sampleCommand) switch (taskDesc.m_sampleCommand)
{ {
case CMD_SAMPLE_INTEGRATE_BODIES: case CMD_SAMPLE_INTEGRATE_BODIES:
{ {
btTransform predictedTrans; btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue; int batchSize = taskDesc.m_sampleValue;
if (batchSize>MAX_NUM_BODIES) if (batchSize>MAX_NUM_BODIES)
{ {
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break; break;
} }
int dmaArraySize = batchSize*sizeof(void*); int dmaArraySize = batchSize*sizeof(void*);
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr); uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16) if (dmaArraySize>=16)
{ {
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
} else } else
{ {
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
} }
for ( int i=0;i<batchSize;i++) for ( int i=0;i<batchSize;i++)
{ {
///DMA rigid body ///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0]; void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i]; void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd); uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody); int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f; float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body) if (body)
{ {
if (body->isActive() && (!body->isStaticOrKinematicObject())) if (body->isActive() && (!body->isStaticOrKinematicObject()))
{ {
body->predictIntegratedTransform(timeStep, predictedTrans); body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans); body->proceedToTransform( predictedTrans);
void* ptr = (void*)localPtr; void* ptr = (void*)localPtr;
// spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress); // spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress);
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
} }
} }
} }
break; break;
} }
case CMD_SAMPLE_PREDICT_MOTION_BODIES: case CMD_SAMPLE_PREDICT_MOTION_BODIES:
{ {
btTransform predictedTrans; btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr; btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue; int batchSize = taskDesc.m_sampleValue;
int dmaArraySize = batchSize*sizeof(void*); int dmaArraySize = batchSize*sizeof(void*);
if (batchSize>MAX_NUM_BODIES) if (batchSize>MAX_NUM_BODIES)
{ {
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n"); spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break; break;
} }
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr); uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize); // spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16) if (dmaArraySize>=16)
{ {
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0); cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
} else } else
{ {
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize); stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
} }
for ( int i=0;i<batchSize;i++) for ( int i=0;i<batchSize;i++)
{ {
///DMA rigid body ///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0]; void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i]; void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd); uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr); // spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody); int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f; float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj); btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body) if (body)
{ {
if (!body->isStaticOrKinematicObject()) if (!body->isStaticOrKinematicObject())
{ {
if (body->isActive()) if (body->isActive())
{ {
body->integrateVelocities( timeStep); body->integrateVelocities( timeStep);
//damping //damping
body->applyDamping(timeStep); body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
void* ptr = (void*)localPtr; void* ptr = (void*)localPtr;
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0); cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1)); cellDmaWaitTagStatusAll(DMA_MASK(1));
} }
} }
} }
} }
break; break;
} }
default: default:
{ {
} }
}; };
} }
#if defined(__CELLOS_LV2__) || defined (LIBSPE2) #if defined(__CELLOS_LV2__) || defined (LIBSPE2)
ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory); ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory);
void* createSampleLocalStoreMemory() void* createSampleLocalStoreMemory()
{ {
return &gLocalStoreMemory; return &gLocalStoreMemory;
} }
#else #else
void* createSampleLocalStoreMemory() void* createSampleLocalStoreMemory()
{ {
return new SampleTask_LocalStoreMemory; return new SampleTask_LocalStoreMemory;
}; };
#endif #endif

108
src/BulletMultiThreaded/SpuSampleTask/SpuSampleTask.h
View File

@@ -1,54 +1,54 @@
/* /*
Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SPU_SAMPLE_TASK_H #ifndef SPU_SAMPLE_TASK_H
#define SPU_SAMPLE_TASK_H #define SPU_SAMPLE_TASK_H
#include "../PlatformDefinitions.h" #include "../PlatformDefinitions.h"
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
enum enum
{ {
CMD_SAMPLE_INTEGRATE_BODIES = 1, CMD_SAMPLE_INTEGRATE_BODIES = 1,
CMD_SAMPLE_PREDICT_MOTION_BODIES CMD_SAMPLE_PREDICT_MOTION_BODIES
}; };
ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_sampleCommand; uint32_t m_sampleCommand;
uint32_t m_taskId; uint32_t m_taskId;
uint64_t m_mainMemoryPtr; uint64_t m_mainMemoryPtr;
int m_sampleValue; int m_sampleValue;
}; };
void processSampleTask(void* userPtr, void* lsMemory); void processSampleTask(void* userPtr, void* lsMemory);
void* createSampleLocalStoreMemory(); void* createSampleLocalStoreMemory();
#endif //SPU_SAMPLE_TASK_H #endif //SPU_SAMPLE_TASK_H

2
src/BulletMultiThreaded/SpuSampleTask/readme.txt
View File

@@ -1 +1 @@
Empty placeholder for future Libspe2 SPU task Empty placeholder for future Libspe2 SPU task

518
src/BulletMultiThreaded/Win32ThreadSupport.cpp
View File

@@ -1,259 +1,259 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "Win32ThreadSupport.h" #include "Win32ThreadSupport.h"
#ifdef USE_WIN32_THREADING #ifdef USE_WIN32_THREADING
#include <windows.h> #include <windows.h>
#include "SpuCollisionTaskProcess.h" #include "SpuCollisionTaskProcess.h"
#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" #include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
///The number of threads should be equal to the number of available cores ///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. ///@todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication ///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
///Setup and initialize SPU/CELL/Libspe2 ///Setup and initialize SPU/CELL/Libspe2
Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo) Win32ThreadSupport::Win32ThreadSupport(const Win32ThreadConstructionInfo & threadConstructionInfo)
{ {
startThreads(threadConstructionInfo); startThreads(threadConstructionInfo);
} }
///cleanup/shutdown Libspe2 ///cleanup/shutdown Libspe2
Win32ThreadSupport::~Win32ThreadSupport() Win32ThreadSupport::~Win32ThreadSupport()
{ {
stopSPU(); stopSPU();
} }
#include <stdio.h> #include <stdio.h>
DWORD WINAPI Thread_no_1( LPVOID lpParam ) DWORD WINAPI Thread_no_1( LPVOID lpParam )
{ {
Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam; Win32ThreadSupport::btSpuStatus* status = (Win32ThreadSupport::btSpuStatus*)lpParam;
while (1) while (1)
{ {
WaitForSingleObject(status->m_eventStartHandle,INFINITE); WaitForSingleObject(status->m_eventStartHandle,INFINITE);
void* userPtr = status->m_userPtr; void* userPtr = status->m_userPtr;
if (userPtr) if (userPtr)
{ {
btAssert(status->m_status); btAssert(status->m_status);
status->m_userThreadFunc(userPtr,status->m_lsMemory); status->m_userThreadFunc(userPtr,status->m_lsMemory);
status->m_status = 2; status->m_status = 2;
SetEvent(status->m_eventCompletetHandle); SetEvent(status->m_eventCompletetHandle);
} else } else
{ {
//exit Thread //exit Thread
status->m_status = 3; status->m_status = 3;
SetEvent(status->m_eventCompletetHandle); SetEvent(status->m_eventCompletetHandle);
printf("Thread with taskId %i with handle %p exiting\n",status->m_taskId, status->m_threadHandle); printf("Thread with taskId %i with handle %p exiting\n",status->m_taskId, status->m_threadHandle);
break; break;
} }
} }
printf("Thread TERMINATED\n"); printf("Thread TERMINATED\n");
return 0; return 0;
} }
///send messages to SPUs ///send messages to SPUs
void Win32ThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId) void Win32ThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId)
{ {
/// gMidphaseSPU.sendRequest(CMD_GATHER_AND_PROCESS_PAIRLIST, (ppu_address_t) &taskDesc); /// 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 ///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) switch (uiCommand)
{ {
case CMD_GATHER_AND_PROCESS_PAIRLIST: case CMD_GATHER_AND_PROCESS_PAIRLIST:
{ {
//#define SINGLE_THREADED 1 //#define SINGLE_THREADED 1
#ifdef SINGLE_THREADED #ifdef SINGLE_THREADED
btSpuStatus& spuStatus = m_activeSpuStatus[0]; btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr=(void*)uiArgument0; spuStatus.m_userPtr=(void*)uiArgument0;
spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory); spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
HANDLE handle =0; HANDLE handle =0;
#else #else
btSpuStatus& spuStatus = m_activeSpuStatus[taskId]; btSpuStatus& spuStatus = m_activeSpuStatus[taskId];
btAssert(taskId>=0); btAssert(taskId>=0);
btAssert(taskId<m_activeSpuStatus.size()); btAssert(taskId<m_activeSpuStatus.size());
spuStatus.m_commandId = uiCommand; spuStatus.m_commandId = uiCommand;
spuStatus.m_status = 1; spuStatus.m_status = 1;
spuStatus.m_userPtr = (void*)uiArgument0; spuStatus.m_userPtr = (void*)uiArgument0;
///fire event to start new task ///fire event to start new task
SetEvent(spuStatus.m_eventStartHandle); SetEvent(spuStatus.m_eventStartHandle);
#endif //CollisionTask_LocalStoreMemory #endif //CollisionTask_LocalStoreMemory
break; break;
} }
default: default:
{ {
///not implemented ///not implemented
btAssert(0); btAssert(0);
} }
}; };
} }
///check for messages from SPUs ///check for messages from SPUs
void Win32ThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1) void Win32ThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
{ {
///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response ///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback' ///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
btAssert(m_activeSpuStatus.size()); btAssert(m_activeSpuStatus.size());
int last = -1; int last = -1;
#ifndef SINGLE_THREADED #ifndef SINGLE_THREADED
DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE); DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE);
btAssert(res != WAIT_FAILED); btAssert(res != WAIT_FAILED);
last = res - WAIT_OBJECT_0; last = res - WAIT_OBJECT_0;
btSpuStatus& spuStatus = m_activeSpuStatus[last]; btSpuStatus& spuStatus = m_activeSpuStatus[last];
btAssert(spuStatus.m_threadHandle); btAssert(spuStatus.m_threadHandle);
btAssert(spuStatus.m_eventCompletetHandle); btAssert(spuStatus.m_eventCompletetHandle);
//WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); //WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
btAssert(spuStatus.m_status > 1); btAssert(spuStatus.m_status > 1);
spuStatus.m_status = 0; spuStatus.m_status = 0;
///need to find an active spu ///need to find an active spu
btAssert(last>=0); btAssert(last>=0);
#else #else
last=0; last=0;
btSpuStatus& spuStatus = m_activeSpuStatus[last]; btSpuStatus& spuStatus = m_activeSpuStatus[last];
#endif //SINGLE_THREADED #endif //SINGLE_THREADED
*puiArgument0 = spuStatus.m_taskId; *puiArgument0 = spuStatus.m_taskId;
*puiArgument1 = spuStatus.m_status; *puiArgument1 = spuStatus.m_status;
} }
void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo) void Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
{ {
m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads); m_activeSpuStatus.resize(threadConstructionInfo.m_numThreads);
m_completeHandles.resize(threadConstructionInfo.m_numThreads); m_completeHandles.resize(threadConstructionInfo.m_numThreads);
for (int i=0;i<threadConstructionInfo.m_numThreads;i++) for (int i=0;i<threadConstructionInfo.m_numThreads;i++)
{ {
printf("starting thread %d\n",i); printf("starting thread %d\n",i);
btSpuStatus& spuStatus = m_activeSpuStatus[i]; btSpuStatus& spuStatus = m_activeSpuStatus[i];
LPSECURITY_ATTRIBUTES lpThreadAttributes=NULL; LPSECURITY_ATTRIBUTES lpThreadAttributes=NULL;
SIZE_T dwStackSize=threadConstructionInfo.m_threadStackSize; SIZE_T dwStackSize=threadConstructionInfo.m_threadStackSize;
LPTHREAD_START_ROUTINE lpStartAddress=&Thread_no_1; LPTHREAD_START_ROUTINE lpStartAddress=&Thread_no_1;
LPVOID lpParameter=&spuStatus; LPVOID lpParameter=&spuStatus;
DWORD dwCreationFlags=0; DWORD dwCreationFlags=0;
LPDWORD lpThreadId=0; LPDWORD lpThreadId=0;
spuStatus.m_userPtr=0; spuStatus.m_userPtr=0;
sprintf(spuStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i); sprintf(spuStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
spuStatus.m_eventStartHandle = CreateEvent(0,false,false,spuStatus.m_eventStartHandleName); spuStatus.m_eventStartHandle = CreateEvent(0,false,false,spuStatus.m_eventStartHandleName);
sprintf(spuStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i); sprintf(spuStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
spuStatus.m_eventCompletetHandle = CreateEvent(0,false,false,spuStatus.m_eventCompletetHandleName); spuStatus.m_eventCompletetHandle = CreateEvent(0,false,false,spuStatus.m_eventCompletetHandleName);
m_completeHandles[i] = spuStatus.m_eventCompletetHandle; m_completeHandles[i] = spuStatus.m_eventCompletetHandle;
HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter, dwCreationFlags,lpThreadId); HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter, dwCreationFlags,lpThreadId);
SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST); SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
//SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL); //SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
SetThreadAffinityMask(handle, 1<<i); SetThreadAffinityMask(handle, 1<<i);
spuStatus.m_taskId = i; spuStatus.m_taskId = i;
spuStatus.m_commandId = 0; spuStatus.m_commandId = 0;
spuStatus.m_status = 0; spuStatus.m_status = 0;
spuStatus.m_threadHandle = handle; spuStatus.m_threadHandle = handle;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc(); spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc; spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf("started thread %d with threadHandle %p\n",i,handle); printf("started thread %d with threadHandle %p\n",i,handle);
} }
} }
void Win32ThreadSupport::startSPU() void Win32ThreadSupport::startSPU()
{ {
} }
///tell the task scheduler we are done with the SPU tasks ///tell the task scheduler we are done with the SPU tasks
void Win32ThreadSupport::stopSPU() void Win32ThreadSupport::stopSPU()
{ {
int i; int i;
for (i=0;i<m_activeSpuStatus.size();i++) for (i=0;i<m_activeSpuStatus.size();i++)
{ {
btSpuStatus& spuStatus = m_activeSpuStatus[i]; btSpuStatus& spuStatus = m_activeSpuStatus[i];
if (spuStatus.m_status>0) if (spuStatus.m_status>0)
{ {
WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
} }
spuStatus.m_userPtr = 0; spuStatus.m_userPtr = 0;
SetEvent(spuStatus.m_eventStartHandle); SetEvent(spuStatus.m_eventStartHandle);
WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE); WaitForSingleObject(spuStatus.m_eventCompletetHandle, INFINITE);
CloseHandle(spuStatus.m_eventCompletetHandle); CloseHandle(spuStatus.m_eventCompletetHandle);
CloseHandle(spuStatus.m_eventStartHandle); CloseHandle(spuStatus.m_eventStartHandle);
CloseHandle(spuStatus.m_threadHandle); CloseHandle(spuStatus.m_threadHandle);
} }
m_activeSpuStatus.clear(); m_activeSpuStatus.clear();
m_completeHandles.clear(); m_completeHandles.clear();
} }
#endif //USE_WIN32_THREADING #endif //USE_WIN32_THREADING

250
src/BulletMultiThreaded/Win32ThreadSupport.h
View File

@@ -1,125 +1,125 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h" #include "PlatformDefinitions.h"
#ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h #ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h
#ifndef WIN32_THREAD_SUPPORT_H #ifndef WIN32_THREAD_SUPPORT_H
#define WIN32_THREAD_SUPPORT_H #define WIN32_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h" #include "btThreadSupportInterface.h"
typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory); typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*Win32lsMemorySetupFunc)(); typedef void* (*Win32lsMemorySetupFunc)();
///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication ///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class Win32ThreadSupport : public btThreadSupportInterface class Win32ThreadSupport : public btThreadSupportInterface
{ {
public: public:
///placeholder, until libspe2 support is there ///placeholder, until libspe2 support is there
struct btSpuStatus struct btSpuStatus
{ {
uint32_t m_taskId; uint32_t m_taskId;
uint32_t m_commandId; uint32_t m_commandId;
uint32_t m_status; uint32_t m_status;
Win32ThreadFunc m_userThreadFunc; Win32ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc
void* m_threadHandle; //this one is calling 'Win32ThreadFunc' void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_eventStartHandle; void* m_eventStartHandle;
char m_eventStartHandleName[32]; char m_eventStartHandleName[32];
void* m_eventCompletetHandle; void* m_eventCompletetHandle;
char m_eventCompletetHandleName[32]; char m_eventCompletetHandleName[32];
}; };
private: private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus; btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles; btAlignedObjectArray<void*> m_completeHandles;
public: public:
///Setup and initialize SPU/CELL/Libspe2 ///Setup and initialize SPU/CELL/Libspe2
struct Win32ThreadConstructionInfo struct Win32ThreadConstructionInfo
{ {
Win32ThreadConstructionInfo(char* uniqueName, Win32ThreadConstructionInfo(char* uniqueName,
Win32ThreadFunc userThreadFunc, Win32ThreadFunc userThreadFunc,
Win32lsMemorySetupFunc lsMemoryFunc, Win32lsMemorySetupFunc lsMemoryFunc,
int numThreads=1, int numThreads=1,
int threadStackSize=65535 int threadStackSize=65535
) )
:m_uniqueName(uniqueName), :m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc), m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc), m_lsMemoryFunc(lsMemoryFunc),
m_numThreads(numThreads), m_numThreads(numThreads),
m_threadStackSize(threadStackSize) m_threadStackSize(threadStackSize)
{ {
} }
char* m_uniqueName; char* m_uniqueName;
Win32ThreadFunc m_userThreadFunc; Win32ThreadFunc m_userThreadFunc;
Win32lsMemorySetupFunc m_lsMemoryFunc; Win32lsMemorySetupFunc m_lsMemoryFunc;
int m_numThreads; int m_numThreads;
int m_threadStackSize; int m_threadStackSize;
}; };
Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo); Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo);
///cleanup/shutdown Libspe2 ///cleanup/shutdown Libspe2
virtual ~Win32ThreadSupport(); virtual ~Win32ThreadSupport();
void startThreads(const Win32ThreadConstructionInfo& threadInfo); void startThreads(const Win32ThreadConstructionInfo& threadInfo);
///send messages to SPUs ///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1); virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs ///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1); 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) ///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(); virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks ///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU(); virtual void stopSPU();
virtual void setNumTasks(int numTasks) virtual void setNumTasks(int numTasks)
{ {
} }
}; };
#endif //WIN32_THREAD_SUPPORT_H #endif //WIN32_THREAD_SUPPORT_H
#endif //USE_WIN32_THREADING #endif //USE_WIN32_THREADING

146
src/BulletMultiThreaded/vectormath2bullet.h
View File

@@ -1,73 +1,73 @@
/* /*
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc. Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
All rights reserved. All rights reserved.
Redistribution and use in source and binary forms, Redistribution and use in source and binary forms,
with or without modification, are permitted provided that the with or without modification, are permitted provided that the
following conditions are met: following conditions are met:
* Redistributions of source code must retain the above copyright * Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer. notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright * Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution. documentation and/or other materials provided with the distribution.
* Neither the name of the Sony Computer Entertainment Inc nor the names * Neither the name of the Sony Computer Entertainment Inc nor the names
of its contributors may be used to endorse or promote products derived of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission. from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 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 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. POSSIBILITY OF SUCH DAMAGE.
*/ */
#ifndef AOS_VECTORMATH_BULLET_CONVERT_H #ifndef AOS_VECTORMATH_BULLET_CONVERT_H
#define AOS_VECTORMATH_BULLET_CONVERT_H #define AOS_VECTORMATH_BULLET_CONVERT_H
#include <vectormath_aos.h> #include <vectormath_aos.h>
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h" #include "LinearMath/btQuaternion.h"
#include "LinearMath/btMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
inline Vectormath::Aos::Vector3 getVmVector3(const btVector3& bulletVec) inline Vectormath::Aos::Vector3 getVmVector3(const btVector3& bulletVec)
{ {
return Vectormath::Aos::Vector3(bulletVec.getX(),bulletVec.getY(),bulletVec.getZ()); return Vectormath::Aos::Vector3(bulletVec.getX(),bulletVec.getY(),bulletVec.getZ());
} }
inline btVector3 getBtVector3(const Vectormath::Aos::Vector3& vmVec) inline btVector3 getBtVector3(const Vectormath::Aos::Vector3& vmVec)
{ {
return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ());
} }
inline btVector3 getBtVector3(const Vectormath::Aos::Point3& vmVec) inline btVector3 getBtVector3(const Vectormath::Aos::Point3& vmVec)
{ {
return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ()); return btVector3(vmVec.getX(),vmVec.getY(),vmVec.getZ());
} }
inline Vectormath::Aos::Quat getVmQuat(const btQuaternion& bulletQuat) inline Vectormath::Aos::Quat getVmQuat(const btQuaternion& bulletQuat)
{ {
Vectormath::Aos::Quat vmQuat(bulletQuat.getX(),bulletQuat.getY(),bulletQuat.getZ(),bulletQuat.getW()); Vectormath::Aos::Quat vmQuat(bulletQuat.getX(),bulletQuat.getY(),bulletQuat.getZ(),bulletQuat.getW());
return vmQuat; return vmQuat;
} }
inline btQuaternion getBtQuat(const Vectormath::Aos::Quat& vmQuat) inline btQuaternion getBtQuat(const Vectormath::Aos::Quat& vmQuat)
{ {
return btQuaternion (vmQuat.getX(),vmQuat.getY(),vmQuat.getZ(),vmQuat.getW()); return btQuaternion (vmQuat.getX(),vmQuat.getY(),vmQuat.getZ(),vmQuat.getW());
} }
inline Vectormath::Aos::Matrix3 getVmMatrix3(const btMatrix3x3& btMat) inline Vectormath::Aos::Matrix3 getVmMatrix3(const btMatrix3x3& btMat)
{ {
Vectormath::Aos::Matrix3 mat( Vectormath::Aos::Matrix3 mat(
getVmVector3(btMat.getColumn(0)), getVmVector3(btMat.getColumn(0)),
getVmVector3(btMat.getColumn(1)), getVmVector3(btMat.getColumn(1)),
getVmVector3(btMat.getColumn(2))); getVmVector3(btMat.getColumn(2)));
return mat; return mat;
} }
#endif //AOS_VECTORMATH_BULLET_CONVERT_H #endif //AOS_VECTORMATH_BULLET_CONVERT_H

5298
src/BulletSoftBody/btSoftBody.cpp
View File

File diff suppressed because it is too large Load Diff

1682
src/BulletSoftBody/btSoftBody.h
View File

File diff suppressed because it is too large Load Diff

738
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp
View File

@@ -1,369 +1,369 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#include "btSoftBodyConcaveCollisionAlgorithm.h" #include "btSoftBodyConcaveCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionShapes/btConcaveShape.h" #include "BulletCollision/CollisionShapes/btConcaveShape.h"
#include "BulletCollision/CollisionDispatch/btManifoldResult.h" #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h" #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletSoftBody/btSoftBody.h" #include "BulletSoftBody/btSoftBody.h"
#define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
: btCollisionAlgorithm(ci), : btCollisionAlgorithm(ci),
m_isSwapped(isSwapped), m_isSwapped(isSwapped),
m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
{ {
} }
btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
{ {
} }
btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
m_dispatcher(dispatcher), m_dispatcher(dispatcher),
m_dispatchInfoPtr(0) m_dispatchInfoPtr(0)
{ {
m_softBody = (btSoftBody*) (isSwapped? body1:body0); m_softBody = (btSoftBody*) (isSwapped? body1:body0);
m_triBody = isSwapped? body0:body1; m_triBody = isSwapped? body0:body1;
// //
// create the manifold from the dispatcher 'manifold pool' // create the manifold from the dispatcher 'manifold pool'
// //
// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
clearCache(); clearCache();
} }
btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
{ {
clearCache(); clearCache();
// m_dispatcher->releaseManifold( m_manifoldPtr ); // m_dispatcher->releaseManifold( m_manifoldPtr );
} }
void btSoftBodyTriangleCallback::clearCache() void btSoftBodyTriangleCallback::clearCache()
{ {
for (int i=0;i<m_shapeCache.size();i++) for (int i=0;i<m_shapeCache.size();i++)
{ {
btTriIndex* tmp = m_shapeCache.getAtIndex(i); btTriIndex* tmp = m_shapeCache.getAtIndex(i);
btAssert(tmp); btAssert(tmp);
btAssert(tmp->m_childShape); btAssert(tmp->m_childShape);
m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary? m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
delete tmp->m_childShape; delete tmp->m_childShape;
} }
m_shapeCache.clear(); m_shapeCache.clear();
} }
void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
{ {
//just for debugging purposes //just for debugging purposes
//printf("triangle %d",m_triangleCount++); //printf("triangle %d",m_triangleCount++);
btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
btCollisionAlgorithmConstructionInfo ci; btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = m_dispatcher; ci.m_dispatcher1 = m_dispatcher;
///debug drawing of the overlapping triangles ///debug drawing of the overlapping triangles
if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
{ {
btVector3 color(255,255,0); btVector3 color(255,255,0);
btTransform& tr = ob->getWorldTransform(); btTransform& tr = ob->getWorldTransform();
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); 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[1]),tr(triangle[2]),color);
m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
} }
btTriIndex triIndex(partId,triangleIndex,0); btTriIndex triIndex(partId,triangleIndex,0);
btHashKey<btTriIndex> triKey(triIndex.getUid()); btHashKey<btTriIndex> triKey(triIndex.getUid());
btTriIndex* shapeIndex = m_shapeCache[triKey]; btTriIndex* shapeIndex = m_shapeCache[triKey];
if (shapeIndex) if (shapeIndex)
{ {
btCollisionShape* tm = shapeIndex->m_childShape; btCollisionShape* tm = shapeIndex->m_childShape;
btAssert(tm); btAssert(tm);
//copy over user pointers to temporary shape //copy over user pointers to temporary shape
tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
btCollisionShape* tmpShape = ob->getCollisionShape(); btCollisionShape* tmpShape = ob->getCollisionShape();
ob->internalSetTemporaryCollisionShape( tm ); ob->internalSetTemporaryCollisionShape( tm );
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm(); colAlgo->~btCollisionAlgorithm();
ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
ob->internalSetTemporaryCollisionShape( tmpShape); ob->internalSetTemporaryCollisionShape( tmpShape);
return; return;
} }
//aabb filter is already applied! //aabb filter is already applied!
//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
// if (m_softBody->getCollisionShape()->getShapeType()== // if (m_softBody->getCollisionShape()->getShapeType()==
{ {
// btVector3 other; // btVector3 other;
btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
normal.normalize(); normal.normalize();
normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
// other+=normal*22.f; // other+=normal*22.f;
btVector3 pts[6] = {triangle[0]+normal, btVector3 pts[6] = {triangle[0]+normal,
triangle[1]+normal, triangle[1]+normal,
triangle[2]+normal, triangle[2]+normal,
triangle[0]-normal, triangle[0]-normal,
triangle[1]-normal, triangle[1]-normal,
triangle[2]-normal}; triangle[2]-normal};
btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
// btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
//btTriangleShape tm(triangle[0],triangle[1],triangle[2]); //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
// tm.setMargin(m_collisionMarginTriangle); // tm.setMargin(m_collisionMarginTriangle);
//copy over user pointers to temporary shape //copy over user pointers to temporary shape
tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer());
btCollisionShape* tmpShape = ob->getCollisionShape(); btCollisionShape* tmpShape = ob->getCollisionShape();
ob->internalSetTemporaryCollisionShape( tm ); ob->internalSetTemporaryCollisionShape( tm );
btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); 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 ///this should use the btDispatcher, so the actual registered algorithm is used
// btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
//m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
colAlgo->~btCollisionAlgorithm(); colAlgo->~btCollisionAlgorithm();
ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
ob->internalSetTemporaryCollisionShape( tmpShape ); ob->internalSetTemporaryCollisionShape( tmpShape );
triIndex.m_childShape = tm; triIndex.m_childShape = tm;
m_shapeCache.insert(triKey,triIndex); m_shapeCache.insert(triKey,triIndex);
} }
} }
void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
m_dispatchInfoPtr = &dispatchInfo; m_dispatchInfoPtr = &dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
m_resultOut = resultOut; m_resultOut = resultOut;
btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax;
m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5); btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
btTransform softTransform; btTransform softTransform;
softTransform.setIdentity(); softTransform.setIdentity();
softTransform.setOrigin(softBodyCenter); softTransform.setOrigin(softBodyCenter);
btTransform convexInTriangleSpace; btTransform convexInTriangleSpace;
convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform; convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax); btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
} }
void btSoftBodyConcaveCollisionAlgorithm::clearCache() void btSoftBodyConcaveCollisionAlgorithm::clearCache()
{ {
m_btSoftBodyTriangleCallback.clearCache(); m_btSoftBodyTriangleCallback.clearCache();
} }
void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
btCollisionObject* triBody = m_isSwapped ? body0 : body1; btCollisionObject* triBody = m_isSwapped ? body0 : body1;
if (triBody->getCollisionShape()->isConcave()) if (triBody->getCollisionShape()->isConcave())
{ {
btCollisionObject* triOb = triBody; btCollisionObject* triOb = triBody;
btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape()); btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
// if (convexBody->getCollisionShape()->isConvex()) // if (convexBody->getCollisionShape()->isConvex())
{ {
btScalar collisionMarginTriangle = concaveShape->getMargin(); btScalar collisionMarginTriangle = concaveShape->getMargin();
// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //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_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
// resultOut->refreshContactPoints(); // resultOut->refreshContactPoints();
} }
} }
} }
btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{ {
(void)resultOut; (void)resultOut;
(void)dispatchInfo; (void)dispatchInfo;
btCollisionObject* convexbody = m_isSwapped ? body1 : body0; btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
btCollisionObject* triBody = m_isSwapped ? body0 : body1; btCollisionObject* triBody = m_isSwapped ? body0 : body1;
//quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) //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 //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... //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(); btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
{ {
return btScalar(1.); return btScalar(1.);
} }
//const btVector3& from = convexbody->m_worldTransform.getOrigin(); //const btVector3& from = convexbody->m_worldTransform.getOrigin();
//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
//todo: only do if the motion exceeds the 'radius' //todo: only do if the motion exceeds the 'radius'
btTransform triInv = triBody->getWorldTransform().inverse(); btTransform triInv = triBody->getWorldTransform().inverse();
btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
struct LocalTriangleSphereCastCallback : public btTriangleCallback struct LocalTriangleSphereCastCallback : public btTriangleCallback
{ {
btTransform m_ccdSphereFromTrans; btTransform m_ccdSphereFromTrans;
btTransform m_ccdSphereToTrans; btTransform m_ccdSphereToTrans;
btTransform m_meshTransform; btTransform m_meshTransform;
btScalar m_ccdSphereRadius; btScalar m_ccdSphereRadius;
btScalar m_hitFraction; btScalar m_hitFraction;
LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
:m_ccdSphereFromTrans(from), :m_ccdSphereFromTrans(from),
m_ccdSphereToTrans(to), m_ccdSphereToTrans(to),
m_ccdSphereRadius(ccdSphereRadius), m_ccdSphereRadius(ccdSphereRadius),
m_hitFraction(hitFraction) m_hitFraction(hitFraction)
{ {
} }
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
{ {
(void)partId; (void)partId;
(void)triangleIndex; (void)triangleIndex;
//do a swept sphere for now //do a swept sphere for now
btTransform ident; btTransform ident;
ident.setIdentity(); ident.setIdentity();
btConvexCast::CastResult castResult; btConvexCast::CastResult castResult;
castResult.m_fraction = m_hitFraction; castResult.m_fraction = m_hitFraction;
btSphereShape pointShape(m_ccdSphereRadius); btSphereShape pointShape(m_ccdSphereRadius);
btTriangleShape triShape(triangle[0],triangle[1],triangle[2]); btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
btVoronoiSimplexSolver simplexSolver; btVoronoiSimplexSolver simplexSolver;
btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver); btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
//local space? //local space?
if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans, if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
ident,ident,castResult)) ident,ident,castResult))
{ {
if (m_hitFraction > castResult.m_fraction) if (m_hitFraction > castResult.m_fraction)
m_hitFraction = castResult.m_fraction; m_hitFraction = castResult.m_fraction;
} }
} }
}; };
if (triBody->getCollisionShape()->isConcave()) if (triBody->getCollisionShape()->isConcave())
{ {
btVector3 rayAabbMin = convexFromLocal.getOrigin(); btVector3 rayAabbMin = convexFromLocal.getOrigin();
rayAabbMin.setMin(convexToLocal.getOrigin()); rayAabbMin.setMin(convexToLocal.getOrigin());
btVector3 rayAabbMax = convexFromLocal.getOrigin(); btVector3 rayAabbMax = convexFromLocal.getOrigin();
rayAabbMax.setMax(convexToLocal.getOrigin()); rayAabbMax.setMax(convexToLocal.getOrigin());
btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0); rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0); rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
btScalar curHitFraction = btScalar(1.); //is this available? btScalar curHitFraction = btScalar(1.); //is this available?
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
convexbody->getCcdSweptSphereRadius(),curHitFraction); convexbody->getCcdSweptSphereRadius(),curHitFraction);
raycastCallback.m_hitFraction = convexbody->getHitFraction(); raycastCallback.m_hitFraction = convexbody->getHitFraction();
btCollisionObject* concavebody = triBody; btCollisionObject* concavebody = triBody;
btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
if (triangleMesh) if (triangleMesh)
{ {
triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
} }
if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
{ {
convexbody->setHitFraction( raycastCallback.m_hitFraction); convexbody->setHitFraction( raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction; return raycastCallback.m_hitFraction;
} }
} }
return btScalar(1.); return btScalar(1.);
} }

306
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
View File

@@ -1,153 +1,153 @@
/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. 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. 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, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: 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. 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. 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. 3. This notice may not be removed or altered from any source distribution.
*/ */
#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H #ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H #define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionShapes/btTriangleCallback.h" #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
class btDispatcher; class btDispatcher;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btSoftBody; class btSoftBody;
class btCollisionShape; class btCollisionShape;
#include "LinearMath/btHashMap.h" #include "LinearMath/btHashMap.h"
#include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS #include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" //for definition of MAX_NUM_PARTS_IN_BITS
struct btTriIndex struct btTriIndex
{ {
int m_PartIdTriangleIndex; int m_PartIdTriangleIndex;
class btCollisionShape* m_childShape; class btCollisionShape* m_childShape;
btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) btTriIndex(int partId,int triangleIndex,btCollisionShape* shape)
{ {
m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex;
m_childShape = shape; m_childShape = shape;
} }
int getTriangleIndex() const int getTriangleIndex() const
{ {
// Get only the lower bits where the triangle index is stored // Get only the lower bits where the triangle index is stored
return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS)));
} }
int getPartId() const int getPartId() const
{ {
// Get only the highest bits where the part index is stored // Get only the highest bits where the part index is stored
return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS)); return (m_PartIdTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
} }
int getUid() const int getUid() const
{ {
return m_PartIdTriangleIndex; return m_PartIdTriangleIndex;
} }
}; };
///For each triangle in the concave mesh that overlaps with the AABB of a soft body (m_softBody), processTriangle is called. ///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 class btSoftBodyTriangleCallback : public btTriangleCallback
{ {
btSoftBody* m_softBody; btSoftBody* m_softBody;
btCollisionObject* m_triBody; btCollisionObject* m_triBody;
btVector3 m_aabbMin; btVector3 m_aabbMin;
btVector3 m_aabbMax ; btVector3 m_aabbMax ;
btManifoldResult* m_resultOut; btManifoldResult* m_resultOut;
btDispatcher* m_dispatcher; btDispatcher* m_dispatcher;
const btDispatcherInfo* m_dispatchInfoPtr; const btDispatcherInfo* m_dispatchInfoPtr;
btScalar m_collisionMarginTriangle; btScalar m_collisionMarginTriangle;
btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache; btHashMap<btHashKey<btTriIndex>,btTriIndex> m_shapeCache;
public: public:
int m_triangleCount; int m_triangleCount;
// btPersistentManifold* m_manifoldPtr; // btPersistentManifold* m_manifoldPtr;
btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); void setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual ~btSoftBodyTriangleCallback(); virtual ~btSoftBodyTriangleCallback();
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
void clearCache(); void clearCache();
SIMD_FORCE_INLINE const btVector3& getAabbMin() const SIMD_FORCE_INLINE const btVector3& getAabbMin() const
{ {
return m_aabbMin; return m_aabbMin;
} }
SIMD_FORCE_INLINE const btVector3& getAabbMax() const SIMD_FORCE_INLINE const btVector3& getAabbMax() const
{ {
return m_aabbMax; return m_aabbMax;
} }
}; };
/// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes. /// btSoftBodyConcaveCollisionAlgorithm supports collision between soft body shapes and (concave) trianges meshes.
class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm class btSoftBodyConcaveCollisionAlgorithm : public btCollisionAlgorithm
{ {
bool m_isSwapped; bool m_isSwapped;
btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback; btSoftBodyTriangleCallback m_btSoftBodyTriangleCallback;
public: public:
btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
virtual ~btSoftBodyConcaveCollisionAlgorithm(); virtual ~btSoftBodyConcaveCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btScalar calculateTimeOfImpact(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) virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{ {
//we don't add any manifolds //we don't add any manifolds
} }
void clearCache(); void clearCache();
struct CreateFunc :public btCollisionAlgorithmCreateFunc struct CreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false); return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,false);
} }
}; };
struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc
{ {
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{ {
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm)); void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSoftBodyConcaveCollisionAlgorithm));
return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true); return new(mem) btSoftBodyConcaveCollisionAlgorithm(ci,body0,body1,true);
} }
}; };
}; };
#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H #endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H

1718
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More