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

4
Demos/AllBulletDemos/DemoEntries.cpp
View File

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

33
Demos/AllBulletDemos/Main.cpp
View File

@@ -52,6 +52,7 @@ namespace
int gDrawAabb;
int gWireFrame;
int gHelpText;
int gDebugConstraints;
int gDebugContacts;
int gDrawTextures=1;
int gDrawShadows=0;
@@ -80,6 +81,14 @@ void setDefaultSettings()
gDrawAabb=0;
gWireFrame=0;
gDebugContacts=0;
//enable constraint debug visualization for first demo, only if user hasn't overridden the setting
if (testSelection>1)
{
gDebugConstraints=0;
} else
{
gDebugConstraints=1;
}
gHelpText = 0;
gDrawTextures=1;
gDrawShadows=0;
@@ -118,6 +127,14 @@ void ResetScene()
void NextScene()
{
testSelection++;
if (testSelection>1)
{
gDebugConstraints=0;
} else
{
gDebugConstraints=1;
}
if(testSelection>23)
testSelection=0;
if (glui)
@@ -216,6 +233,13 @@ void SimulationLoop()
{
demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_NoHelpText);
}
if (gDebugConstraints)
{
demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits);
} else
{
demo->setDebugMode(demo->getDebugMode() & (~btIDebugDraw::DBG_DrawConstraints+btIDebugDraw::DBG_DrawConstraintLimits));
}
if (gDebugContacts)
{
demo->setDebugMode(demo->getDebugMode() |btIDebugDraw::DBG_DrawContactPoints);
@@ -281,6 +305,14 @@ void SimulationLoop()
}
if (testSelection != testIndex)
{
if (testSelection>1)
{
gDebugConstraints=0;
} else
{
gDebugConstraints=1;
}
testIndex = testSelection;
if (demo->getDynamicsWorld() && demo->getDynamicsWorld()->getDebugDrawer())
delete demo->getDynamicsWorld()->getDebugDrawer();
@@ -464,6 +496,7 @@ int main(int argc, char** argv)
glui->add_checkbox_to_panel(drawPanel, "AABBs", &gDrawAabb);
glui->add_checkbox_to_panel(drawPanel, "Wireframe", &gWireFrame);
glui->add_checkbox_to_panel(drawPanel, "Contacts", &gDebugContacts);
glui->add_checkbox_to_panel(drawPanel, "Constraints", &gDebugConstraints);
glui->add_checkbox_to_panel(drawPanel, "Textures", &gDrawTextures);
glui->add_checkbox_to_panel(drawPanel, "Shadows", &gDrawShadows);

2
Demos/ColladaDemo/ColladaDemo.cpp
View File

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

22
Demos/RagdollDemo/RagdollDemo.cpp
View File

@@ -16,6 +16,8 @@ subject to the following restrictions:
Written by: Marten Svanfeldt
*/
#define CONSTRAINT_DEBUG_SIZE 0.2f
#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
@@ -197,6 +199,8 @@ public:
hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
m_joints[JOINT_PELVIS_SPINE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
@@ -206,6 +210,8 @@ public:
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
m_joints[JOINT_SPINE_HEAD] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
@@ -215,6 +221,8 @@ public:
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_LEFT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
localA.setIdentity(); localB.setIdentity();
@@ -223,6 +231,8 @@ public:
hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
@@ -232,6 +242,8 @@ public:
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
coneC->setLimit(M_PI_4, M_PI_4, 0);
m_joints[JOINT_RIGHT_HIP] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
localA.setIdentity(); localB.setIdentity();
@@ -240,6 +252,8 @@ public:
hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_KNEE] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
@@ -248,6 +262,8 @@ public:
localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[JOINT_LEFT_SHOULDER] = coneC;
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
@@ -258,6 +274,8 @@ public:
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_LEFT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
@@ -268,6 +286,8 @@ public:
coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
coneC->setLimit(M_PI_2, M_PI_2, 0);
m_joints[JOINT_RIGHT_SHOULDER] = coneC;
coneC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
localA.setIdentity(); localB.setIdentity();
@@ -277,6 +297,8 @@ public:
// hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
m_joints[JOINT_RIGHT_ELBOW] = hingeC;
hingeC->setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
}

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
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btDbvtBroadphase implementation by Nathanael Presson
#ifndef BT_DBVT_BROADPHASE_H
#define BT_DBVT_BROADPHASE_H
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
//
// Compile time config
//
#define DBVT_BP_PROFILE 0
//#define DBVT_BP_SORTPAIRS 1
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h"
#endif
//
// btDbvtProxy
//
struct btDbvtProxy : btBroadphaseProxy
{
/* Fields */
//btDbvtAabbMm aabb;
btDbvtNode* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
{
links[0]=links[1]=0;
}
};
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
struct btDbvtBroadphase : btBroadphaseInterface
{
/* Config */
enum {
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */
};
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup?
#if DBVT_BP_PROFILE
btClock m_clock;
struct {
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
unsigned long m_cleanup;
unsigned long m_jobcount;
} m_profiling;
#endif
/* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0);
~btDbvtBroadphase();
void collide(btDispatcher* dispatcher);
void optimize();
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats();
static void benchmark(btBroadphaseInterface*);
void performDeferredRemoval(btDispatcher* dispatcher);
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher);
};
#endif
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///btDbvtBroadphase implementation by Nathanael Presson
#ifndef BT_DBVT_BROADPHASE_H
#define BT_DBVT_BROADPHASE_H
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
//
// Compile time config
//
#define DBVT_BP_PROFILE 0
//#define DBVT_BP_SORTPAIRS 1
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
#define DBVT_BP_MARGIN (btScalar)0.05
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h"
#endif
//
// btDbvtProxy
//
struct btDbvtProxy : btBroadphaseProxy
{
/* Fields */
//btDbvtAabbMm aabb;
btDbvtNode* leaf;
btDbvtProxy* links[2];
int stage;
/* ctor */
btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
{
links[0]=links[1]=0;
}
};
typedef btAlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
///The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see btDbvt).
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
struct btDbvtBroadphase : btBroadphaseInterface
{
/* Config */
enum {
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */
};
/* Fields */
btDbvt m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
btOverlappingPairCache* m_paircache; // Pair cache
btScalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage
int m_fupdates; // % of fixed updates per frame
int m_dupdates; // % of dynamic updates per frame
int m_cupdates; // % of cleanup updates per frame
int m_newpairs; // Number of pairs created
int m_fixedleft; // Fixed optimization left
unsigned m_updates_call; // Number of updates call
unsigned m_updates_done; // Number of updates done
btScalar m_updates_ratio; // m_updates_done/m_updates_call
int m_pid; // Parse id
int m_cid; // Cleanup index
int m_gid; // Gen id
bool m_releasepaircache; // Release pair cache on delete
bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup?
#if DBVT_BP_PROFILE
btClock m_clock;
struct {
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
unsigned long m_cleanup;
unsigned long m_jobcount;
} m_profiling;
#endif
/* Methods */
btDbvtBroadphase(btOverlappingPairCache* paircache=0);
~btDbvtBroadphase();
void collide(btDispatcher* dispatcher);
void optimize();
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy);
void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0));
virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const;
void calculateOverlappingPairs(btDispatcher* dispatcher);
btOverlappingPairCache* getOverlappingPairCache();
const btOverlappingPairCache* getOverlappingPairCache() const;
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats();
static void benchmark(btBroadphaseInterface*);
void performDeferredRemoval(btDispatcher* dispatcher);
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher);
};
#endif

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

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

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

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

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

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

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

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

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

@@ -1,47 +1,47 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btActivatingCollisionAlgorithm.h"
#include "btCollisionDispatcher.h"
#include "btCollisionObject.h"
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
:btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
}
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1)
:btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
// {
// m_colObj0 = colObj0;
// m_colObj1 = colObj1;
//
// m_colObj0->activate();
// m_colObj1->activate();
// }
}
btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
{
// m_colObj0->activate();
// m_colObj1->activate();
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btActivatingCollisionAlgorithm.h"
#include "btCollisionDispatcher.h"
#include "btCollisionObject.h"
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci)
:btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
}
btActivatingCollisionAlgorithm::btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1)
:btCollisionAlgorithm(ci)
//,
//m_colObj0(0),
//m_colObj1(0)
{
// if (ci.m_dispatcher1->needsCollision(colObj0,colObj1))
// {
// m_colObj0 = colObj0;
// m_colObj1 = colObj1;
//
// m_colObj0->activate();
// m_colObj1->activate();
// }
}
btActivatingCollisionAlgorithm::~btActivatingCollisionAlgorithm()
{
// m_colObj0->activate();
// m_colObj1->activate();
}

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

@@ -1,36 +1,36 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
#define __BT_ACTIVATING_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
{
// btCollisionObject* m_colObj0;
// btCollisionObject* m_colObj1;
public:
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1);
virtual ~btActivatingCollisionAlgorithm();
};
#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BT_ACTIVATING_COLLISION_ALGORITHM_H
#define __BT_ACTIVATING_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
///This class is not enabled yet (work-in-progress) to more aggressively activate objects.
class btActivatingCollisionAlgorithm : public btCollisionAlgorithm
{
// btCollisionObject* m_colObj0;
// btCollisionObject* m_colObj1;
public:
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci);
btActivatingCollisionAlgorithm (const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* colObj0,btCollisionObject* colObj1);
virtual ~btActivatingCollisionAlgorithm();
};
#endif //__BT_ACTIVATING_COLLISION_ALGORITHM_H

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

@@ -1,85 +1,85 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btBoxBoxCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btBoxBoxDetector.h"
#define USE_PERSISTENT_CONTACTS 1
btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
: btActivatingCollisionAlgorithm(ci,obj0,obj1),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
{
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
m_ownManifold = true;
}
}
btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
btCollisionObject* col0 = body0;
btCollisionObject* col1 = body1;
btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
#ifndef USE_PERSISTENT_CONTACTS
m_manifoldPtr->clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform();
btBoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#ifdef USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
{
//not yet
return 1.f;
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btBoxBoxCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "btBoxBoxDetector.h"
#define USE_PERSISTENT_CONTACTS 1
btBoxBoxCollisionAlgorithm::btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
: btActivatingCollisionAlgorithm(ci,obj0,obj1),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
{
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
m_ownManifold = true;
}
}
btBoxBoxCollisionAlgorithm::~btBoxBoxCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void btBoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
btCollisionObject* col0 = body0;
btCollisionObject* col1 = body1;
btBoxShape* box0 = (btBoxShape*)col0->getCollisionShape();
btBoxShape* box1 = (btBoxShape*)col1->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
#ifndef USE_PERSISTENT_CONTACTS
m_manifoldPtr->clearManifold();
#endif //USE_PERSISTENT_CONTACTS
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform();
input.m_transformB = body1->getWorldTransform();
btBoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#ifdef USE_PERSISTENT_CONTACTS
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
#endif //USE_PERSISTENT_CONTACTS
}
btScalar btBoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* /*body0*/,btCollisionObject* /*body1*/,const btDispatcherInfo& /*dispatchInfo*/,btManifoldResult* /*resultOut*/)
{
//not yet
return 1.f;
}

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

@@ -1,66 +1,66 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BOX_BOX__COLLISION_ALGORITHM_H
#define BOX_BOX__COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
///box-box collision detection
class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
virtual ~btBoxBoxCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
}
};
};
#endif //BOX_BOX__COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BOX_BOX__COLLISION_ALGORITHM_H
#define BOX_BOX__COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
///box-box collision detection
class btBoxBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btBoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btBoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
virtual ~btBoxBoxCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
int bbsize = sizeof(btBoxBoxCollisionAlgorithm);
void* ptr = ci.m_dispatcher1->allocateCollisionAlgorithm(bbsize);
return new(ptr) btBoxBoxCollisionAlgorithm(0,ci,body0,body1);
}
};
};
#endif //BOX_BOX__COLLISION_ALGORITHM_H

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
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BOX_BOX_DETECTOR_H
#define BOX_BOX_DETECTOR_H
class btBoxShape;
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
/// btBoxBoxDetector wraps the ODE box-box collision detector
/// re-distributed under the Zlib license with permission from Russell L. Smith
struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
{
btBoxShape* m_box1;
btBoxShape* m_box2;
public:
btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
virtual ~btBoxBoxDetector() {};
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
};
#endif //BT_BOX_BOX_DETECTOR_H
/*
* Box-Box collision detection re-distributed under the ZLib license with permission from Russell L. Smith
* Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
* All rights reserved. Email: russ@q12.org Web: www.q12.org
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BOX_BOX_DETECTOR_H
#define BOX_BOX_DETECTOR_H
class btBoxShape;
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
/// btBoxBoxDetector wraps the ODE box-box collision detector
/// re-distributed under the Zlib license with permission from Russell L. Smith
struct btBoxBoxDetector : public btDiscreteCollisionDetectorInterface
{
btBoxShape* m_box1;
btBoxShape* m_box2;
public:
btBoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
virtual ~btBoxBoxDetector() {};
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults=false);
};
#endif //BT_BOX_BOX_DETECTOR_H

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

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

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

@@ -1,84 +1,84 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H
#define CONVEX_PLANE_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
public:
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
CreateFunc()
: m_numPerturbationIterations(3),
m_minimumPointsPerturbationThreshold(3)
{
}
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} else
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
}
}
};
};
#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H
#define CONVEX_PLANE_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
public:
btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
int m_numPerturbationIterations;
int m_minimumPointsPerturbationThreshold;
CreateFunc()
: m_numPerturbationIterations(3),
m_minimumPointsPerturbationThreshold(3)
{
}
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} else
{
return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
}
}
};
};
#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H

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

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

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

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

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

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

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

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

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

@@ -1,75 +1,75 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
#define SPHERE_BOX_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
public:
btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
virtual ~btSphereBoxCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
} else
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
}
}
};
};
#endif //SPHERE_BOX_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H
#define SPHERE_BOX_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
#include "LinearMath/btVector3.h"
/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
class btSphereBoxCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_isSwapped;
public:
btSphereBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
virtual ~btSphereBoxCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
btScalar getSphereDistance( btCollisionObject* boxObj,btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius );
btScalar getSpherePenetration( btCollisionObject* boxObj, btVector3& v3PointOnBox, btVector3& v3PointOnSphere, const btVector3& v3SphereCenter, btScalar fRadius, const btVector3& aabbMin, const btVector3& aabbMax);
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm));
if (!m_swapped)
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false);
} else
{
return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true);
}
}
};
};
#endif //SPHERE_BOX_COLLISION_ALGORITHM_H

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

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

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

@@ -1,66 +1,66 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h"
class btPersistentManifold;
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
}
};
};
#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "btCollisionDispatcher.h"
class btPersistentManifold;
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm));
return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1);
}
};
};
#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H

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

@@ -1,84 +1,84 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSphereTriangleCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "SphereTriangleDetector.h"
btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
: btActivatingCollisionAlgorithm(ci,col0,col1),
m_ownManifold(false),
m_manifoldPtr(mf),
m_swapped(swapped)
{
if (!m_manifoldPtr)
{
m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
m_ownManifold = true;
}
}
btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
btCollisionObject* sphereObj = m_swapped? col1 : col0;
btCollisionObject* triObj = m_swapped? col0 : col1;
btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds
input.m_transformA = sphereObj->getWorldTransform();
input.m_transformB = triObj->getWorldTransform();
bool swapResults = m_swapped;
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
if (m_ownManifold)
resultOut->refreshContactPoints();
}
btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
(void)col0;
(void)col1;
//not yet
return btScalar(1.);
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSphereTriangleCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "SphereTriangleDetector.h"
btSphereTriangleCollisionAlgorithm::btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1,bool swapped)
: btActivatingCollisionAlgorithm(ci,col0,col1),
m_ownManifold(false),
m_manifoldPtr(mf),
m_swapped(swapped)
{
if (!m_manifoldPtr)
{
m_manifoldPtr = m_dispatcher->getNewManifold(col0,col1);
m_ownManifold = true;
}
}
btSphereTriangleCollisionAlgorithm::~btSphereTriangleCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void btSphereTriangleCollisionAlgorithm::processCollision (btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
btCollisionObject* sphereObj = m_swapped? col1 : col0;
btCollisionObject* triObj = m_swapped? col0 : col1;
btSphereShape* sphere = (btSphereShape*)sphereObj->getCollisionShape();
btTriangleShape* triangle = (btTriangleShape*)triObj->getCollisionShape();
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold());
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared = btScalar(1e30);///@todo: tighter bounds
input.m_transformA = sphereObj->getWorldTransform();
input.m_transformB = triObj->getWorldTransform();
bool swapResults = m_swapped;
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw,swapResults);
if (m_ownManifold)
resultOut->refreshContactPoints();
}
btScalar btSphereTriangleCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
(void)resultOut;
(void)dispatchInfo;
(void)col0;
(void)col1;
//not yet
return btScalar(1.);
}

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

@@ -1,69 +1,69 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_swapped;
public:
btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereTriangleCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
}
};
};
#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H
#include "btActivatingCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
#include "btCollisionDispatcher.h"
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereTriangleCollisionAlgorithm : public btActivatingCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_swapped;
public:
btSphereTriangleCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool swapped);
btSphereTriangleCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btActivatingCollisionAlgorithm(ci) {}
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr && m_ownManifold)
{
manifoldArray.push_back(m_manifoldPtr);
}
}
virtual ~btSphereTriangleCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm));
return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped);
}
};
};
#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H

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

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

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

@@ -1,118 +1,118 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CAPSULE_SHAPE_H
#define BT_CAPSULE_SHAPE_H
#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
class btCapsuleShape : public btConvexInternalShape
{
protected:
int m_upAxis;
protected:
///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
public:
btCapsuleShape(btScalar radius,btScalar height);
///CollisionShape Interface
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
/// btConvexShape Interface
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btVector3 halfExtents(getRadius(),getRadius(),getRadius());
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
halfExtents += btVector3(getMargin(),getMargin(),getMargin());
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
virtual const char* getName()const
{
return "CapsuleShape";
}
int getUpAxis() const
{
return m_upAxis;
}
btScalar getRadius() const
{
int radiusAxis = (m_upAxis+2)%3;
return m_implicitShapeDimensions[radiusAxis];
}
btScalar getHalfHeight() const
{
return m_implicitShapeDimensions[m_upAxis];
}
};
///btCapsuleShapeX represents a capsule around the Z axis
///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
class btCapsuleShapeX : public btCapsuleShape
{
public:
btCapsuleShapeX(btScalar radius,btScalar height);
//debugging
virtual const char* getName()const
{
return "CapsuleX";
}
};
///btCapsuleShapeZ represents a capsule around the Z axis
///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
class btCapsuleShapeZ : public btCapsuleShape
{
public:
btCapsuleShapeZ(btScalar radius,btScalar height);
//debugging
virtual const char* getName()const
{
return "CapsuleZ";
}
};
#endif //BT_CAPSULE_SHAPE_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CAPSULE_SHAPE_H
#define BT_CAPSULE_SHAPE_H
#include "btConvexInternalShape.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
///The btCapsuleShape represents a capsule around the Y axis, there is also the btCapsuleShapeX aligned around the X axis and btCapsuleShapeZ around the Z axis.
///The total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
///The btCapsuleShape is a convex hull of two spheres. The btMultiSphereShape is a more general collision shape that takes the convex hull of multiple sphere, so it can also represent a capsule when just using two spheres.
class btCapsuleShape : public btConvexInternalShape
{
protected:
int m_upAxis;
protected:
///only used for btCapsuleShapeZ and btCapsuleShapeX subclasses.
btCapsuleShape() : btConvexInternalShape() {m_shapeType = CAPSULE_SHAPE_PROXYTYPE;};
public:
btCapsuleShape(btScalar radius,btScalar height);
///CollisionShape Interface
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
/// btConvexShape Interface
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const;
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const;
virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
btVector3 halfExtents(getRadius(),getRadius(),getRadius());
halfExtents[m_upAxis] = getRadius() + getHalfHeight();
halfExtents += btVector3(getMargin(),getMargin(),getMargin());
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),abs_b[1].dot(halfExtents),abs_b[2].dot(halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
virtual const char* getName()const
{
return "CapsuleShape";
}
int getUpAxis() const
{
return m_upAxis;
}
btScalar getRadius() const
{
int radiusAxis = (m_upAxis+2)%3;
return m_implicitShapeDimensions[radiusAxis];
}
btScalar getHalfHeight() const
{
return m_implicitShapeDimensions[m_upAxis];
}
};
///btCapsuleShapeX represents a capsule around the Z axis
///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
class btCapsuleShapeX : public btCapsuleShape
{
public:
btCapsuleShapeX(btScalar radius,btScalar height);
//debugging
virtual const char* getName()const
{
return "CapsuleX";
}
};
///btCapsuleShapeZ represents a capsule around the Z axis
///the total height is height+2*radius, so the height is just the height between the center of each 'sphere' of the capsule caps.
class btCapsuleShapeZ : public btCapsuleShape
{
public:
btCapsuleShapeZ(btScalar radius,btScalar height);
//debugging
virtual const char* getName()const
{
return "CapsuleZ";
}
};
#endif //BT_CAPSULE_SHAPE_H

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

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

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

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

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

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

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

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

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

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

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

@@ -1,34 +1,34 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#ifndef MATERIAL_H
#define MATERIAL_H
// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
class btMaterial
{
// public members so that materials can change due to world events
public:
btScalar m_friction;
btScalar m_restitution;
int pad[2];
btMaterial(){}
btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
};
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#ifndef MATERIAL_H
#define MATERIAL_H
// Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties
class btMaterial
{
// public members so that materials can change due to world events
public:
btScalar m_friction;
btScalar m_restitution;
int pad[2];
btMaterial(){}
btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; }
};
#endif // MATERIAL_H

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

@@ -1,45 +1,45 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
///Obtains the material for a specific triangle
const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
{
const unsigned char * materialBase = 0;
int numMaterials;
PHY_ScalarType materialType;
int materialStride;
const unsigned char * triangleMaterialBase = 0;
int numTriangles;
int triangleMaterialStride;
PHY_ScalarType triangleType;
((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
&triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
// return the pointer to the place with the friction for the triangle
// TODO: This depends on whether it's a moving mesh or not
// BUG IN GIMPACT
//return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
return (matVal);
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#include "BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.h"
//#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
///Obtains the material for a specific triangle
const btMaterial * btMultimaterialTriangleMeshShape::getMaterialProperties(int partID, int triIndex)
{
const unsigned char * materialBase = 0;
int numMaterials;
PHY_ScalarType materialType;
int materialStride;
const unsigned char * triangleMaterialBase = 0;
int numTriangles;
int triangleMaterialStride;
PHY_ScalarType triangleType;
((btTriangleIndexVertexMaterialArray*)m_meshInterface)->getLockedReadOnlyMaterialBase(&materialBase, numMaterials, materialType, materialStride,
&triangleMaterialBase, numTriangles, triangleMaterialStride, triangleType, partID);
// return the pointer to the place with the friction for the triangle
// TODO: This depends on whether it's a moving mesh or not
// BUG IN GIMPACT
//return (btScalar*)(&materialBase[triangleMaterialBase[(triIndex-1) * triangleMaterialStride] * materialStride]);
int * matInd = (int *)(&(triangleMaterialBase[(triIndex * triangleMaterialStride)]));
btMaterial *matVal = (btMaterial *)(&(materialBase[*matInd * materialStride]));
return (matVal);
}

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

@@ -1,123 +1,123 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#include "btBvhTriangleMeshShape.h"
#include "btMaterial.h"
///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
{
btAlignedObjectArray <btMaterial*> m_materialList;
int ** m_triangleMaterials;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
{
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{
m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase,
numverts,
type,
stride,
&indexbase,
indexstride,
numfaces,
indicestype,
i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
}
}
///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
{
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{
m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase,
numverts,
type,
stride,
&indexbase,
indexstride,
numfaces,
indicestype,
i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
}
}
virtual ~btMultimaterialTriangleMeshShape()
{
/*
for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
{
btAlignedFree(m_materialValues[i]);
m_materialLookup[i] = NULL;
}
btAlignedFree(m_materialValues);
m_materialLookup = NULL;
*/
}
//debugging
virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";}
///Obtains the material for a specific triangle
const btMaterial * getMaterialProperties(int partID, int triIndex);
}
;
#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
/// This file was created by Alex Silverman
#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H
#include "btBvhTriangleMeshShape.h"
#include "btMaterial.h"
///The BvhTriangleMaterialMeshShape extends the btBvhTriangleMeshShape. Its main contribution is the interface into a material array, which allows per-triangle friction and restitution.
ATTRIBUTE_ALIGNED16(class) btMultimaterialTriangleMeshShape : public btBvhTriangleMeshShape
{
btAlignedObjectArray <btMaterial*> m_materialList;
int ** m_triangleMaterials;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
{
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{
m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase,
numverts,
type,
stride,
&indexbase,
indexstride,
numfaces,
indicestype,
i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces, 16));
}
}
///optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
{
m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
PHY_ScalarType type;
int stride;
const unsigned char *indexbase;
int indexstride;
int numfaces;
PHY_ScalarType indicestype;
//m_materialLookup = (int**)(btAlignedAlloc(sizeof(int*) * meshInterface->getNumSubParts(), 16));
for(int i = 0; i < meshInterface->getNumSubParts(); i++)
{
m_meshInterface->getLockedReadOnlyVertexIndexBase(
&vertexbase,
numverts,
type,
stride,
&indexbase,
indexstride,
numfaces,
indicestype,
i);
//m_materialLookup[i] = (int*)(btAlignedAlloc(sizeof(int) * numfaces * 2, 16));
}
}
virtual ~btMultimaterialTriangleMeshShape()
{
/*
for(int i = 0; i < m_meshInterface->getNumSubParts(); i++)
{
btAlignedFree(m_materialValues[i]);
m_materialLookup[i] = NULL;
}
btAlignedFree(m_materialValues);
m_materialLookup = NULL;
*/
}
//debugging
virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";}
///Obtains the material for a specific triangle
const btMaterial * getMaterialProperties(int partID, int triIndex);
}
;
#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H

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

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

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

@@ -1,62 +1,62 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
{
btVector3 m_localScaling;
btBvhTriangleMeshShape* m_bvhTriMeshShape;
public:
btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
virtual ~btScaledBvhTriangleMeshShape();
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
btBvhTriangleMeshShape* getChildShape()
{
return m_bvhTriMeshShape;
}
const btBvhTriangleMeshShape* getChildShape() const
{
return m_bvhTriMeshShape;
}
//debugging
virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";}
};
#endif //BVH_TRIANGLE_MESH_SHAPE_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
///The btScaledBvhTriangleMeshShape allows to instance a scaled version of an existing btBvhTriangleMeshShape.
///Note that each btBvhTriangleMeshShape still can have its own local scaling, independent from this btScaledBvhTriangleMeshShape 'localScaling'
ATTRIBUTE_ALIGNED16(class) btScaledBvhTriangleMeshShape : public btConcaveShape
{
btVector3 m_localScaling;
btBvhTriangleMeshShape* m_bvhTriMeshShape;
public:
btScaledBvhTriangleMeshShape(btBvhTriangleMeshShape* childShape,const btVector3& localScaling);
virtual ~btScaledBvhTriangleMeshShape();
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const;
virtual void setLocalScaling(const btVector3& scaling);
virtual const btVector3& getLocalScaling() const;
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const;
virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const;
btBvhTriangleMeshShape* getChildShape()
{
return m_bvhTriMeshShape;
}
const btBvhTriangleMeshShape* getChildShape() const
{
return m_bvhTriMeshShape;
}
//debugging
virtual const char* getName()const {return "SCALEDBVHTRIANGLEMESH";}
};
#endif //BVH_TRIANGLE_MESH_SHAPE_H

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

@@ -1,164 +1,164 @@
/*
btbtShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btShapeHull.h"
#include "LinearMath/btConvexHull.h"
#define NUM_UNITSPHERE_POINTS 42
static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
{
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
};
btShapeHull::btShapeHull (const btConvexShape* shape)
{
m_shape = shape;
m_vertices.clear ();
m_indices.clear();
m_numIndices = 0;
}
btShapeHull::~btShapeHull ()
{
m_indices.clear();
m_vertices.clear ();
}
bool
btShapeHull::buildHull (btScalar /*margin*/)
{
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = m_shape->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
{
btVector3 norm;
m_shape->getPreferredPenetrationDirection(i,norm);
btUnitSpherePoints[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i;
for (i = 0; i < numSampleDirections; i++)
{
supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
}
HullDesc hd;
hd.mFlags = QF_TRIANGLES;
hd.mVcount = static_cast<unsigned int>(numSampleDirections);
#ifdef BT_USE_DOUBLE_PRECISION
hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof(btVector3);
#else
hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof (btVector3);
#endif
HullLibrary hl;
HullResult hr;
if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
{
return false;
}
m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
{
m_vertices[i] = hr.m_OutputVertices[i];
}
m_numIndices = hr.mNumIndices;
m_indices.resize(static_cast<int>(m_numIndices));
for (i = 0; i < static_cast<int>(m_numIndices); i++)
{
m_indices[i] = hr.m_Indices[i];
}
// free temporary hull result that we just copied
hl.ReleaseResult (hr);
return true;
}
int
btShapeHull::numTriangles () const
{
return static_cast<int>(m_numIndices / 3);
}
int
btShapeHull::numVertices () const
{
return m_vertices.size ();
}
int
btShapeHull::numIndices () const
{
return static_cast<int>(m_numIndices);
}
/*
btbtShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btShapeHull.h"
#include "LinearMath/btConvexHull.h"
#define NUM_UNITSPHERE_POINTS 42
static btVector3 btUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] =
{
btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)),
btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)),
btVector3(btScalar(-0.276388) , btScalar(-0.850649),btScalar(-0.447219)),
btVector3(btScalar(-0.894426) , btScalar(-0.000000),btScalar(-0.447216)),
btVector3(btScalar(-0.276388) , btScalar(0.850649),btScalar(-0.447220)),
btVector3(btScalar(0.723608) , btScalar(0.525725),btScalar(-0.447219)),
btVector3(btScalar(0.276388) , btScalar(-0.850649),btScalar(0.447220)),
btVector3(btScalar(-0.723608) , btScalar(-0.525725),btScalar(0.447219)),
btVector3(btScalar(-0.723608) , btScalar(0.525725),btScalar(0.447219)),
btVector3(btScalar(0.276388) , btScalar(0.850649),btScalar(0.447219)),
btVector3(btScalar(0.894426) , btScalar(0.000000),btScalar(0.447216)),
btVector3(btScalar(-0.000000) , btScalar(0.000000),btScalar(1.000000)),
btVector3(btScalar(0.425323) , btScalar(-0.309011),btScalar(-0.850654)),
btVector3(btScalar(-0.162456) , btScalar(-0.499995),btScalar(-0.850654)),
btVector3(btScalar(0.262869) , btScalar(-0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.425323) , btScalar(0.309011),btScalar(-0.850654)),
btVector3(btScalar(0.850648) , btScalar(-0.000000),btScalar(-0.525736)),
btVector3(btScalar(-0.525730) , btScalar(-0.000000),btScalar(-0.850652)),
btVector3(btScalar(-0.688190) , btScalar(-0.499997),btScalar(-0.525736)),
btVector3(btScalar(-0.162456) , btScalar(0.499995),btScalar(-0.850654)),
btVector3(btScalar(-0.688190) , btScalar(0.499997),btScalar(-0.525736)),
btVector3(btScalar(0.262869) , btScalar(0.809012),btScalar(-0.525738)),
btVector3(btScalar(0.951058) , btScalar(0.309013),btScalar(0.000000)),
btVector3(btScalar(0.951058) , btScalar(-0.309013),btScalar(0.000000)),
btVector3(btScalar(0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(0.000000) , btScalar(-1.000000),btScalar(0.000000)),
btVector3(btScalar(-0.587786) , btScalar(-0.809017),btScalar(0.000000)),
btVector3(btScalar(-0.951058) , btScalar(-0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.951058) , btScalar(0.309013),btScalar(-0.000000)),
btVector3(btScalar(-0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(-0.000000) , btScalar(1.000000),btScalar(-0.000000)),
btVector3(btScalar(0.587786) , btScalar(0.809017),btScalar(-0.000000)),
btVector3(btScalar(0.688190) , btScalar(-0.499997),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(-0.809012),btScalar(0.525738)),
btVector3(btScalar(-0.850648) , btScalar(0.000000),btScalar(0.525736)),
btVector3(btScalar(-0.262869) , btScalar(0.809012),btScalar(0.525738)),
btVector3(btScalar(0.688190) , btScalar(0.499997),btScalar(0.525736)),
btVector3(btScalar(0.525730) , btScalar(0.000000),btScalar(0.850652)),
btVector3(btScalar(0.162456) , btScalar(-0.499995),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(-0.309011),btScalar(0.850654)),
btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)),
btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654))
};
btShapeHull::btShapeHull (const btConvexShape* shape)
{
m_shape = shape;
m_vertices.clear ();
m_indices.clear();
m_numIndices = 0;
}
btShapeHull::~btShapeHull ()
{
m_indices.clear();
m_vertices.clear ();
}
bool
btShapeHull::buildHull (btScalar /*margin*/)
{
int numSampleDirections = NUM_UNITSPHERE_POINTS;
{
int numPDA = m_shape->getNumPreferredPenetrationDirections();
if (numPDA)
{
for (int i=0;i<numPDA;i++)
{
btVector3 norm;
m_shape->getPreferredPenetrationDirection(i,norm);
btUnitSpherePoints[numSampleDirections] = norm;
numSampleDirections++;
}
}
}
btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2];
int i;
for (i = 0; i < numSampleDirections; i++)
{
supportPoints[i] = m_shape->localGetSupportingVertex(btUnitSpherePoints[i]);
}
HullDesc hd;
hd.mFlags = QF_TRIANGLES;
hd.mVcount = static_cast<unsigned int>(numSampleDirections);
#ifdef BT_USE_DOUBLE_PRECISION
hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof(btVector3);
#else
hd.mVertices = &supportPoints[0];
hd.mVertexStride = sizeof (btVector3);
#endif
HullLibrary hl;
HullResult hr;
if (hl.CreateConvexHull (hd, hr) == QE_FAIL)
{
return false;
}
m_vertices.resize (static_cast<int>(hr.mNumOutputVertices));
for (i = 0; i < static_cast<int>(hr.mNumOutputVertices); i++)
{
m_vertices[i] = hr.m_OutputVertices[i];
}
m_numIndices = hr.mNumIndices;
m_indices.resize(static_cast<int>(m_numIndices));
for (i = 0; i < static_cast<int>(m_numIndices); i++)
{
m_indices[i] = hr.m_Indices[i];
}
// free temporary hull result that we just copied
hl.ReleaseResult (hr);
return true;
}
int
btShapeHull::numTriangles () const
{
return static_cast<int>(m_numIndices / 3);
}
int
btShapeHull::numVertices () const
{
return m_vertices.size ();
}
int
btShapeHull::numIndices () const
{
return static_cast<int>(m_numIndices);
}

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

@@ -1,56 +1,56 @@
/*
btShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _SHAPE_HULL_H
#define _SHAPE_HULL_H
#include "LinearMath/btAlignedObjectArray.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices.
///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object.
///It approximates the convex hull using the supporting vertex of 42 directions.
class btShapeHull
{
public:
btShapeHull (const btConvexShape* shape);
~btShapeHull ();
bool buildHull (btScalar margin);
int numTriangles () const;
int numVertices () const;
int numIndices () const;
const btVector3* getVertexPointer() const
{
return &m_vertices[0];
}
const unsigned int* getIndexPointer() const
{
return &m_indices[0];
}
protected:
btAlignedObjectArray<btVector3> m_vertices;
btAlignedObjectArray<unsigned int> m_indices;
unsigned int m_numIndices;
const btConvexShape* m_shape;
};
#endif //_SHAPE_HULL_H
/*
btShapeHull implemented by John McCutchan.
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _SHAPE_HULL_H
#define _SHAPE_HULL_H
#include "LinearMath/btAlignedObjectArray.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
///The btShapeHull class takes a btConvexShape, builds a simplified convex hull using btConvexHull and provides triangle indices and vertices.
///It can be useful for to simplify a complex convex object and for visualization of a non-polyhedral convex object.
///It approximates the convex hull using the supporting vertex of 42 directions.
class btShapeHull
{
public:
btShapeHull (const btConvexShape* shape);
~btShapeHull ();
bool buildHull (btScalar margin);
int numTriangles () const;
int numVertices () const;
int numIndices () const;
const btVector3* getVertexPointer() const
{
return &m_vertices[0];
}
const unsigned int* getIndexPointer() const
{
return &m_indices[0];
}
protected:
btAlignedObjectArray<btVector3> m_vertices;
btAlignedObjectArray<unsigned int> m_indices;
unsigned int m_numIndices;
const btConvexShape* m_shape;
};
#endif //_SHAPE_HULL_H

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

@@ -1,35 +1,35 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btTriangleBuffer.h"
void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex)
{
btTriangle tri;
tri.m_vertex0 = triangle[0];
tri.m_vertex1 = triangle[1];
tri.m_vertex2 = triangle[2];
tri.m_partId = partId;
tri.m_triangleIndex = triangleIndex;
m_triangleBuffer.push_back(tri);
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btTriangleBuffer.h"
void btTriangleBuffer::processTriangle(btVector3* triangle,int partId,int triangleIndex)
{
btTriangle tri;
tri.m_vertex0 = triangle[0];
tri.m_vertex1 = triangle[1];
tri.m_vertex2 = triangle[2];
tri.m_partId = partId;
tri.m_triangleIndex = triangleIndex;
m_triangleBuffer.push_back(tri);
}

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

@@ -1,69 +1,69 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRIANGLE_BUFFER_H
#define BT_TRIANGLE_BUFFER_H
#include "btTriangleCallback.h"
#include "LinearMath/btAlignedObjectArray.h"
struct btTriangle
{
btVector3 m_vertex0;
btVector3 m_vertex1;
btVector3 m_vertex2;
int m_partId;
int m_triangleIndex;
};
///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
///Example usage of this class:
/// btTriangleBuffer triBuf;
/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
/// for (int i=0;i<triBuf.getNumTriangles();i++)
/// {
/// const btTriangle& tri = triBuf.getTriangle(i);
/// //do something useful here with the triangle
/// }
class btTriangleBuffer : public btTriangleCallback
{
btAlignedObjectArray<btTriangle> m_triangleBuffer;
public:
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
int getNumTriangles() const
{
return int(m_triangleBuffer.size());
}
const btTriangle& getTriangle(int index) const
{
return m_triangleBuffer[index];
}
void clearBuffer()
{
m_triangleBuffer.clear();
}
};
#endif //BT_TRIANGLE_BUFFER_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRIANGLE_BUFFER_H
#define BT_TRIANGLE_BUFFER_H
#include "btTriangleCallback.h"
#include "LinearMath/btAlignedObjectArray.h"
struct btTriangle
{
btVector3 m_vertex0;
btVector3 m_vertex1;
btVector3 m_vertex2;
int m_partId;
int m_triangleIndex;
};
///The btTriangleBuffer callback can be useful to collect and store overlapping triangles between AABB and concave objects that support 'processAllTriangles'
///Example usage of this class:
/// btTriangleBuffer triBuf;
/// concaveShape->processAllTriangles(&triBuf,aabbMin, aabbMax);
/// for (int i=0;i<triBuf.getNumTriangles();i++)
/// {
/// const btTriangle& tri = triBuf.getTriangle(i);
/// //do something useful here with the triangle
/// }
class btTriangleBuffer : public btTriangleCallback
{
btAlignedObjectArray<btTriangle> m_triangleBuffer;
public:
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
int getNumTriangles() const
{
return int(m_triangleBuffer.size());
}
const btTriangle& getTriangle(int index) const
{
return m_triangleBuffer[index];
}
void clearBuffer()
{
m_triangleBuffer.clear();
}
};
#endif //BT_TRIANGLE_BUFFER_H

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

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

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

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

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

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

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

@@ -1,54 +1,54 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
EPA Copyright (c) Ricardo Padrela 2006
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
{
(void)debugDraw;
(void)v;
(void)simplexSolver;
const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin());
btGjkEpaSolver2::sResults results;
if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB,
guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
return true;
}
return false;
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
EPA Copyright (c) Ricardo Padrela 2006
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
{
(void)debugDraw;
(void)v;
(void)simplexSolver;
const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(transformA.getOrigin()-transformB.getOrigin());
btGjkEpaSolver2::sResults results;
if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB,
guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
return true;
}
return false;
}

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

@@ -1,43 +1,43 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CHARACTER_CONTROLLER_INTERFACE_H
#define CHARACTER_CONTROLLER_INTERFACE_H
#include "LinearMath/btVector3.h"
class btCollisionShape;
class btRigidBody;
class btCollisionWorld;
class btCharacterControllerInterface
{
public:
btCharacterControllerInterface () {};
virtual ~btCharacterControllerInterface () {};
virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void reset () = 0;
virtual void warp (const btVector3& origin) = 0;
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual bool onGround () const = 0;
};
#endif
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CHARACTER_CONTROLLER_INTERFACE_H
#define CHARACTER_CONTROLLER_INTERFACE_H
#include "LinearMath/btVector3.h"
class btCollisionShape;
class btRigidBody;
class btCollisionWorld;
class btCharacterControllerInterface
{
public:
btCharacterControllerInterface () {};
virtual ~btCharacterControllerInterface () {};
virtual void setWalkDirection(const btVector3& walkDirection) = 0;
virtual void reset () = 0;
virtual void warp (const btVector3& origin) = 0;
virtual void preStep ( btCollisionWorld* collisionWorld) = 0;
virtual void playerStep (btCollisionWorld* collisionWorld, btScalar dt) = 0;
virtual bool canJump () const = 0;
virtual void jump () = 0;
virtual bool onGround () const = 0;
};
#endif

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

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

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

@@ -1,116 +1,116 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef KINEMATIC_CHARACTER_CONTROLLER_H
#define KINEMATIC_CHARACTER_CONTROLLER_H
#include "LinearMath/btVector3.h"
#include "btCharacterControllerInterface.h"
class btCollisionShape;
class btRigidBody;
class btCollisionWorld;
class btCollisionDispatcher;
class btPairCachingGhostObject;
///btKinematicCharacterController is an object that supports a sliding motion in a world.
///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
class btKinematicCharacterController : public btCharacterControllerInterface
{
protected:
btScalar m_halfHeight;
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_turnAngle;
btScalar m_stepHeight;
btScalar m_addedMargin;//@todo: remove this and fix the code
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
bool m_useGhostObjectSweepTest;
int m_upAxis;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
bool recoverFromPenetration (btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public:
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~btKinematicCharacterController ();
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
virtual void setWalkDirection(const btVector3& walkDirection)
{
m_walkDirection = walkDirection;
}
void reset ();
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
void setFallSpeed (btScalar fallSpeed);
void setJumpSpeed (btScalar jumpSpeed);
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
m_useGhostObjectSweepTest = useGhostObjectSweepTest;
}
bool onGround () const;
};
#endif // KINEMATIC_CHARACTER_CONTROLLER_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2008 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef KINEMATIC_CHARACTER_CONTROLLER_H
#define KINEMATIC_CHARACTER_CONTROLLER_H
#include "LinearMath/btVector3.h"
#include "btCharacterControllerInterface.h"
class btCollisionShape;
class btRigidBody;
class btCollisionWorld;
class btCollisionDispatcher;
class btPairCachingGhostObject;
///btKinematicCharacterController is an object that supports a sliding motion in a world.
///It uses a ghost object and convex sweep test to test for upcoming collisions. This is combined with discrete collision detection to recover from penetrations.
///Interaction between btKinematicCharacterController and dynamic rigid bodies needs to be explicity implemented by the user.
class btKinematicCharacterController : public btCharacterControllerInterface
{
protected:
btScalar m_halfHeight;
btPairCachingGhostObject* m_ghostObject;
btConvexShape* m_convexShape;//is also in m_ghostObject, but it needs to be convex, so we store it here to avoid upcast
btScalar m_fallSpeed;
btScalar m_jumpSpeed;
btScalar m_maxJumpHeight;
btScalar m_turnAngle;
btScalar m_stepHeight;
btScalar m_addedMargin;//@todo: remove this and fix the code
///this is the desired walk direction, set by the user
btVector3 m_walkDirection;
//some internal variables
btVector3 m_currentPosition;
btScalar m_currentStepOffset;
btVector3 m_targetPosition;
///keep track of the contact manifolds
btManifoldArray m_manifoldArray;
bool m_touchingContact;
btVector3 m_touchingNormal;
bool m_useGhostObjectSweepTest;
int m_upAxis;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
bool recoverFromPenetration (btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public:
btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~btKinematicCharacterController ();
void setUpAxis (int axis)
{
if (axis < 0)
axis = 0;
if (axis > 2)
axis = 2;
m_upAxis = axis;
}
virtual void setWalkDirection(const btVector3& walkDirection)
{
m_walkDirection = walkDirection;
}
void reset ();
void warp (const btVector3& origin);
void preStep ( btCollisionWorld* collisionWorld);
void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
void setFallSpeed (btScalar fallSpeed);
void setJumpSpeed (btScalar jumpSpeed);
void setMaxJumpHeight (btScalar maxJumpHeight);
bool canJump () const;
void jump ();
btPairCachingGhostObject* getGhostObject();
void setUseGhostSweepTest(bool useGhostObjectSweepTest)
{
m_useGhostObjectSweepTest = useGhostObjectSweepTest;
}
bool onGround () const;
};
#endif // KINEMATIC_CHARACTER_CONTROLLER_H

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

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

@@ -1,179 +1,179 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#define USE_SIMD 1
#endif //
#ifdef USE_SIMD
struct btSimdScalar
{
SIMD_FORCE_INLINE btSimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
}
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
:m_vec128(v128)
{
}
union
{
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
btScalar m_unusedPadding;
};
SIMD_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
SIMD_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
SIMD_FORCE_INLINE operator __m128()
{
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}
#else
#define btSimdScalar btScalar
#endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btScalar m_angularFactor;
btScalar m_invMass;
btScalar m_friction;
btRigidBody* m_originalBody;
btVector3 m_pushVelocity;
//btVector3 m_turnVelocity;
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
if (m_originalBody)
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
}
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{
if (m_originalBody)
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
//if (m_invMass)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
/*
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
*/
void writebackVelocity(btScalar timeStep=0)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //BT_SOLVER_BODY_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#define USE_SIMD 1
#endif //
#ifdef USE_SIMD
struct btSimdScalar
{
SIMD_FORCE_INLINE btSimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
}
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
:m_vec128(v128)
{
}
union
{
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
btScalar m_unusedPadding;
};
SIMD_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
SIMD_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
SIMD_FORCE_INLINE operator __m128()
{
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}
#else
#define btSimdScalar btScalar
#endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btScalar m_angularFactor;
btScalar m_invMass;
btScalar m_friction;
btRigidBody* m_originalBody;
btVector3 m_pushVelocity;
//btVector3 m_turnVelocity;
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
if (m_originalBody)
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
}
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{
if (m_originalBody)
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
//if (m_invMass)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
/*
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
*/
void writebackVelocity(btScalar timeStep=0)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //BT_SOLVER_BODY_H

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

@@ -19,7 +19,7 @@ subject to the following restrictions:
static btRigidBody s_fixed(0, 0,0);
#define DEFAULT_DEBUGDRAW_SIZE btScalar(5.f)
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
:m_userConstraintType(-1),

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

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

@@ -1,136 +1,136 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DYNAMICS_WORLD_H
#define BT_DYNAMICS_WORLD_H
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class btTypedConstraint;
class btRaycastVehicle;
class btConstraintSolver;
class btDynamicsWorld;
class btCharacterControllerInterface;
/// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
enum btDynamicsWorldType
{
BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
class btDynamicsWorld : public btCollisionWorld
{
protected:
btInternalTickCallback m_internalTickCallback;
void* m_worldUserInfo;
btContactSolverInfo m_solverInfo;
public:
btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
{
}
virtual ~btDynamicsWorld()
{
}
///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
virtual void debugDrawWorld() = 0;
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false)
{
(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
}
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;}
virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;}
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0;
virtual btVector3 getGravity () const = 0;
virtual void synchronizeMotionStates() = 0;
virtual void addRigidBody(btRigidBody* body) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
virtual btConstraintSolver* getConstraintSolver() = 0;
virtual int getNumConstraints() const { return 0; }
virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
virtual btDynamicsWorldType getWorldType() const=0;
virtual void clearForces() = 0;
/// Set the callback for when an internal tick (simulation substep) happens, optional user info
void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
{
m_internalTickCallback = cb;
m_worldUserInfo = worldUserInfo;
}
void setWorldUserInfo(void* worldUserInfo)
{
m_worldUserInfo = worldUserInfo;
}
void* getWorldUserInfo() const
{
return m_worldUserInfo;
}
btContactSolverInfo& getSolverInfo()
{
return m_solverInfo;
}
};
#endif //BT_DYNAMICS_WORLD_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_DYNAMICS_WORLD_H
#define BT_DYNAMICS_WORLD_H
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class btTypedConstraint;
class btRaycastVehicle;
class btConstraintSolver;
class btDynamicsWorld;
class btCharacterControllerInterface;
/// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
enum btDynamicsWorldType
{
BT_SIMPLE_DYNAMICS_WORLD=1,
BT_DISCRETE_DYNAMICS_WORLD=2,
BT_CONTINUOUS_DYNAMICS_WORLD=3
};
///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
class btDynamicsWorld : public btCollisionWorld
{
protected:
btInternalTickCallback m_internalTickCallback;
void* m_worldUserInfo;
btContactSolverInfo m_solverInfo;
public:
btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
{
}
virtual ~btDynamicsWorld()
{
}
///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
virtual void debugDrawWorld() = 0;
virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false)
{
(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
}
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;}
virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;}
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0;
virtual btVector3 getGravity () const = 0;
virtual void synchronizeMotionStates() = 0;
virtual void addRigidBody(btRigidBody* body) = 0;
virtual void removeRigidBody(btRigidBody* body) = 0;
virtual void setConstraintSolver(btConstraintSolver* solver) = 0;
virtual btConstraintSolver* getConstraintSolver() = 0;
virtual int getNumConstraints() const { return 0; }
virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; }
virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; }
virtual btDynamicsWorldType getWorldType() const=0;
virtual void clearForces() = 0;
/// Set the callback for when an internal tick (simulation substep) happens, optional user info
void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0)
{
m_internalTickCallback = cb;
m_worldUserInfo = worldUserInfo;
}
void setWorldUserInfo(void* worldUserInfo)
{
m_worldUserInfo = worldUserInfo;
}
void* getWorldUserInfo() const
{
return m_worldUserInfo;
}
btContactSolverInfo& getSolverInfo()
{
return m_solverInfo;
}
};
#endif //BT_DYNAMICS_WORLD_H

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

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

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

186
src/BulletMultiThreaded/SequentialThreadSupport.cpp
View File

@@ -1,93 +1,93 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SequentialThreadSupport.h"
#include "SpuCollisionTaskProcess.h"
#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo)
{
startThreads(threadConstructionInfo);
}
///cleanup/shutdown Libspe2
SequentialThreadSupport::~SequentialThreadSupport()
{
stopSPU();
}
#include <stdio.h>
///send messages to SPUs
void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId)
{
switch (uiCommand)
{
case CMD_GATHER_AND_PROCESS_PAIRLIST:
{
btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr=(void*)uiArgument0;
spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
}
break;
default:
{
///not implemented
btAssert(0 && "Not implemented");
}
};
}
///check for messages from SPUs
void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
{
btAssert(m_activeSpuStatus.size());
btSpuStatus& spuStatus = m_activeSpuStatus[0];
*puiArgument0 = spuStatus.m_taskId;
*puiArgument1 = spuStatus.m_status;
}
void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo)
{
m_activeSpuStatus.resize(1);
printf("STS: Not starting any threads\n");
btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr = 0;
spuStatus.m_taskId = 0;
spuStatus.m_commandId = 0;
spuStatus.m_status = 0;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName);
}
void SequentialThreadSupport::startSPU()
{
}
void SequentialThreadSupport::stopSPU()
{
m_activeSpuStatus.clear();
}
void SequentialThreadSupport::setNumTasks(int numTasks)
{
printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks);
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SequentialThreadSupport.h"
#include "SpuCollisionTaskProcess.h"
#include "SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
SequentialThreadSupport::SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo)
{
startThreads(threadConstructionInfo);
}
///cleanup/shutdown Libspe2
SequentialThreadSupport::~SequentialThreadSupport()
{
stopSPU();
}
#include <stdio.h>
///send messages to SPUs
void SequentialThreadSupport::sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t taskId)
{
switch (uiCommand)
{
case CMD_GATHER_AND_PROCESS_PAIRLIST:
{
btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr=(void*)uiArgument0;
spuStatus.m_userThreadFunc(spuStatus.m_userPtr,spuStatus.m_lsMemory);
}
break;
default:
{
///not implemented
btAssert(0 && "Not implemented");
}
};
}
///check for messages from SPUs
void SequentialThreadSupport::waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1)
{
btAssert(m_activeSpuStatus.size());
btSpuStatus& spuStatus = m_activeSpuStatus[0];
*puiArgument0 = spuStatus.m_taskId;
*puiArgument1 = spuStatus.m_status;
}
void SequentialThreadSupport::startThreads(SequentialThreadConstructionInfo& threadConstructionInfo)
{
m_activeSpuStatus.resize(1);
printf("STS: Not starting any threads\n");
btSpuStatus& spuStatus = m_activeSpuStatus[0];
spuStatus.m_userPtr = 0;
spuStatus.m_taskId = 0;
spuStatus.m_commandId = 0;
spuStatus.m_status = 0;
spuStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
spuStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf("STS: Created local store at %p for task %s\n", spuStatus.m_lsMemory, threadConstructionInfo.m_uniqueName);
}
void SequentialThreadSupport::startSPU()
{
}
void SequentialThreadSupport::stopSPU()
{
m_activeSpuStatus.clear();
}
void SequentialThreadSupport::setNumTasks(int numTasks)
{
printf("SequentialThreadSupport::setNumTasks(%d) is not implemented and has no effect\n",numTasks);
}

174
src/BulletMultiThreaded/SequentialThreadSupport.h
View File

@@ -1,87 +1,87 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h"
#ifndef SEQUENTIAL_THREAD_SUPPORT_H
#define SEQUENTIAL_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h"
typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*SequentiallsMemorySetupFunc)();
///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface
///This is useful for debugging and porting SPU Tasks to other platforms.
class SequentialThreadSupport : public btThreadSupportInterface
{
public:
struct btSpuStatus
{
uint32_t m_taskId;
uint32_t m_commandId;
uint32_t m_status;
SequentialThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc
};
private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles;
public:
struct SequentialThreadConstructionInfo
{
SequentialThreadConstructionInfo (char* uniqueName,
SequentialThreadFunc userThreadFunc,
SequentiallsMemorySetupFunc lsMemoryFunc
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc)
{
}
char* m_uniqueName;
SequentialThreadFunc m_userThreadFunc;
SequentiallsMemorySetupFunc m_lsMemoryFunc;
};
SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo);
virtual ~SequentialThreadSupport();
void startThreads(SequentialThreadConstructionInfo& threadInfo);
///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU();
virtual void setNumTasks(int numTasks);
};
#endif //SEQUENTIAL_THREAD_SUPPORT_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h"
#ifndef SEQUENTIAL_THREAD_SUPPORT_H
#define SEQUENTIAL_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h"
typedef void (*SequentialThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*SequentiallsMemorySetupFunc)();
///The SequentialThreadSupport is a portable non-parallel implementation of the btThreadSupportInterface
///This is useful for debugging and porting SPU Tasks to other platforms.
class SequentialThreadSupport : public btThreadSupportInterface
{
public:
struct btSpuStatus
{
uint32_t m_taskId;
uint32_t m_commandId;
uint32_t m_status;
SequentialThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using SequentiallsMemorySetupFunc
};
private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles;
public:
struct SequentialThreadConstructionInfo
{
SequentialThreadConstructionInfo (char* uniqueName,
SequentialThreadFunc userThreadFunc,
SequentiallsMemorySetupFunc lsMemoryFunc
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc)
{
}
char* m_uniqueName;
SequentialThreadFunc m_userThreadFunc;
SequentiallsMemorySetupFunc m_lsMemoryFunc;
};
SequentialThreadSupport(SequentialThreadConstructionInfo& threadConstructionInfo);
virtual ~SequentialThreadSupport();
void startThreads(SequentialThreadConstructionInfo& threadInfo);
///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU();
virtual void setNumTasks(int numTasks);
};
#endif //SEQUENTIAL_THREAD_SUPPORT_H

138
src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.cpp
View File

@@ -1,69 +1,69 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuContactManifoldCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
btAssert(0);
}
btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
btAssert(0);
return 1.f;
}
#ifndef __SPU__
SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
:btCollisionAlgorithm(ci)
#ifdef USE_SEPDISTANCE_UTIL
,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc())
#endif //USE_SEPDISTANCE_UTIL
{
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_shapeType0 = body0->getCollisionShape()->getShapeType();
m_shapeType1 = body1->getCollisionShape()->getShapeType();
m_collisionMargin0 = body0->getCollisionShape()->getMargin();
m_collisionMargin1 = body1->getCollisionShape()->getMargin();
m_collisionObject0 = body0;
m_collisionObject1 = body1;
if (body0->getCollisionShape()->isPolyhedral())
{
btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape();
m_shapeDimensions0 = convex0->getImplicitShapeDimensions();
}
if (body1->getCollisionShape()->isPolyhedral())
{
btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape();
m_shapeDimensions1 = convex1->getImplicitShapeDimensions();
}
}
#endif //__SPU__
SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm()
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuContactManifoldCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
void SpuContactManifoldCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
btAssert(0);
}
btScalar SpuContactManifoldCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
btAssert(0);
return 1.f;
}
#ifndef __SPU__
SpuContactManifoldCollisionAlgorithm::SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
:btCollisionAlgorithm(ci)
#ifdef USE_SEPDISTANCE_UTIL
,m_sepDistance(body0->getCollisionShape()->getAngularMotionDisc(),body1->getCollisionShape()->getAngularMotionDisc())
#endif //USE_SEPDISTANCE_UTIL
{
m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
m_shapeType0 = body0->getCollisionShape()->getShapeType();
m_shapeType1 = body1->getCollisionShape()->getShapeType();
m_collisionMargin0 = body0->getCollisionShape()->getMargin();
m_collisionMargin1 = body1->getCollisionShape()->getMargin();
m_collisionObject0 = body0;
m_collisionObject1 = body1;
if (body0->getCollisionShape()->isPolyhedral())
{
btPolyhedralConvexShape* convex0 = (btPolyhedralConvexShape*)body0->getCollisionShape();
m_shapeDimensions0 = convex0->getImplicitShapeDimensions();
}
if (body1->getCollisionShape()->isPolyhedral())
{
btPolyhedralConvexShape* convex1 = (btPolyhedralConvexShape*)body1->getCollisionShape();
m_shapeDimensions1 = convex1->getImplicitShapeDimensions();
}
}
#endif //__SPU__
SpuContactManifoldCollisionAlgorithm::~SpuContactManifoldCollisionAlgorithm()
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}

240
src/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h
View File

@@ -1,120 +1,120 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btTransformUtil.h"
class btPersistentManifold;
//#define USE_SEPDISTANCE_UTIL 1
/// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU.
ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
{
btVector3 m_shapeDimensions0;
btVector3 m_shapeDimensions1;
btPersistentManifold* m_manifoldPtr;
int m_shapeType0;
int m_shapeType1;
float m_collisionMargin0;
float m_collisionMargin1;
btCollisionObject* m_collisionObject0;
btCollisionObject* m_collisionObject1;
public:
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
#ifdef USE_SEPDISTANCE_UTIL
btConvexSeparatingDistanceUtil m_sepDistance;
#endif //USE_SEPDISTANCE_UTIL
virtual ~SpuContactManifoldCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr)
manifoldArray.push_back(m_manifoldPtr);
}
btPersistentManifold* getContactManifoldPtr()
{
return m_manifoldPtr;
}
btCollisionObject* getCollisionObject0()
{
return m_collisionObject0;
}
btCollisionObject* getCollisionObject1()
{
return m_collisionObject1;
}
int getShapeType0() const
{
return m_shapeType0;
}
int getShapeType1() const
{
return m_shapeType1;
}
float getCollisionMargin0() const
{
return m_collisionMargin0;
}
float getCollisionMargin1() const
{
return m_collisionMargin1;
}
const btVector3& getShapeDimensions0() const
{
return m_shapeDimensions0;
}
const btVector3& getShapeDimensions1() const
{
return m_shapeDimensions1;
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
}
};
};
#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#define SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
#include "LinearMath/btTransformUtil.h"
class btPersistentManifold;
//#define USE_SEPDISTANCE_UTIL 1
/// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU.
ATTRIBUTE_ALIGNED16(class) SpuContactManifoldCollisionAlgorithm : public btCollisionAlgorithm
{
btVector3 m_shapeDimensions0;
btVector3 m_shapeDimensions1;
btPersistentManifold* m_manifoldPtr;
int m_shapeType0;
int m_shapeType1;
float m_collisionMargin0;
float m_collisionMargin1;
btCollisionObject* m_collisionObject0;
btCollisionObject* m_collisionObject1;
public:
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
SpuContactManifoldCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
#ifdef USE_SEPDISTANCE_UTIL
btConvexSeparatingDistanceUtil m_sepDistance;
#endif //USE_SEPDISTANCE_UTIL
virtual ~SpuContactManifoldCollisionAlgorithm();
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_manifoldPtr)
manifoldArray.push_back(m_manifoldPtr);
}
btPersistentManifold* getContactManifoldPtr()
{
return m_manifoldPtr;
}
btCollisionObject* getCollisionObject0()
{
return m_collisionObject0;
}
btCollisionObject* getCollisionObject1()
{
return m_collisionObject1;
}
int getShapeType0() const
{
return m_shapeType0;
}
int getShapeType1() const
{
return m_shapeType1;
}
float getCollisionMargin0() const
{
return m_collisionMargin0;
}
float getCollisionMargin1() const
{
return m_collisionMargin1;
}
const btVector3& getShapeDimensions0() const
{
return m_shapeDimensions0;
}
const btVector3& getShapeDimensions1() const
{
return m_shapeDimensions1;
}
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm));
return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1);
}
};
};
#endif //SPU_CONTACTMANIFOLD_COLLISION_ALGORITHM_H

418
src/BulletMultiThreaded/SpuFakeDma.cpp
View File

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

138
src/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h
View File

@@ -1,69 +1,69 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_GATHERING_COLLISION__DISPATCHER_H
#define SPU_GATHERING_COLLISION__DISPATCHER_H
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
///Tuning value to optimized SPU utilization
///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.
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
#define SPU_BATCHSIZE_BROADPHASE_PAIRS 64
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024
class SpuCollisionTaskProcess;
///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection
///Time of Impact, Closest Points and Penetration Depth.
class SpuGatheringCollisionDispatcher : public btCollisionDispatcher
{
SpuCollisionTaskProcess* m_spuCollisionTaskProcess;
protected:
class btThreadSupportInterface* m_threadInterface;
unsigned int m_maxNumOutstandingTasks;
public:
//can be used by SPU collision algorithms
SpuCollisionTaskProcess* getSpuCollisionTaskProcess()
{
return m_spuCollisionTaskProcess;
}
SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
virtual ~SpuGatheringCollisionDispatcher();
bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
};
#endif //SPU_GATHERING_COLLISION__DISPATCHER_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_GATHERING_COLLISION__DISPATCHER_H
#define SPU_GATHERING_COLLISION__DISPATCHER_H
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
///Tuning value to optimized SPU utilization
///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.
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 8
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 16
#define SPU_BATCHSIZE_BROADPHASE_PAIRS 64
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 128
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 256
//#define SPU_BATCHSIZE_BROADPHASE_PAIRS 1024
class SpuCollisionTaskProcess;
///SpuGatheringCollisionDispatcher can use SPU to gather and calculate collision detection
///Time of Impact, Closest Points and Penetration Depth.
class SpuGatheringCollisionDispatcher : public btCollisionDispatcher
{
SpuCollisionTaskProcess* m_spuCollisionTaskProcess;
protected:
class btThreadSupportInterface* m_threadInterface;
unsigned int m_maxNumOutstandingTasks;
public:
//can be used by SPU collision algorithms
SpuCollisionTaskProcess* getSpuCollisionTaskProcess()
{
return m_spuCollisionTaskProcess;
}
SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration);
virtual ~SpuGatheringCollisionDispatcher();
bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1);
virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) ;
};
#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.
All rights reserved.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BOX_H__
#define __BOX_H__
#ifndef PE_REF
#define PE_REF(a) a&
#endif
#include <math.h>
#include <vectormath_aos.h>
using namespace Vectormath::Aos;
enum FeatureType { F, E, V };
//----------------------------------------------------------------------------
// Box
//----------------------------------------------------------------------------
///The Box is an internal class used by the boxBoxDistance calculation.
class Box
{
public:
Vector3 half;
inline Box()
{}
inline Box(PE_REF(Vector3) half_);
inline Box(float hx, float hy, float hz);
inline void Set(PE_REF(Vector3) half_);
inline void Set(float hx, float hy, float hz);
inline Vector3 GetAABB(const Matrix3& rotation) const;
};
inline
Box::Box(PE_REF(Vector3) half_)
{
Set(half_);
}
inline
Box::Box(float hx, float hy, float hz)
{
Set(hx, hy, hz);
}
inline
void
Box::Set(PE_REF(Vector3) half_)
{
half = half_;
}
inline
void
Box::Set(float hx, float hy, float hz)
{
half = Vector3(hx, hy, hz);
}
inline
Vector3
Box::GetAABB(const Matrix3& rotation) const
{
return absPerElem(rotation) * half;
}
//-------------------------------------------------------------------------------------------------
// BoxPoint
//-------------------------------------------------------------------------------------------------
///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation.
class BoxPoint
{
public:
BoxPoint() : localPoint(0.0f) {}
Point3 localPoint;
FeatureType featureType;
int featureIdx;
inline void setVertexFeature(int plusX, int plusY, int plusZ);
inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1);
inline void setFaceFeature(int dim, int plus);
inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const;
inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const;
inline void getFaceFeature(int & dim, int & plus) const;
};
inline
void
BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ)
{
featureType = V;
featureIdx = plusX << 2 | plusY << 1 | plusZ;
}
inline
void
BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1)
{
featureType = E;
if (dim0 > dim1) {
featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0;
} else {
featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1;
}
}
inline
void
BoxPoint::setFaceFeature(int dim, int plus)
{
featureType = F;
featureIdx = plus << 2 | dim;
}
inline
void
BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const
{
plusX = featureIdx >> 2;
plusY = featureIdx >> 1 & 1;
plusZ = featureIdx & 1;
}
inline
void
BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const
{
plus0 = featureIdx >> 5;
dim0 = featureIdx >> 3 & 3;
plus1 = featureIdx >> 2 & 1;
dim1 = featureIdx & 3;
}
inline
void
BoxPoint::getFaceFeature(int & dim, int & plus) const
{
plus = featureIdx >> 2;
dim = featureIdx & 3;
}
#endif /* __BOX_H__ */
/*
Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
All rights reserved.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BOX_H__
#define __BOX_H__
#ifndef PE_REF
#define PE_REF(a) a&
#endif
#include <math.h>
#include <vectormath_aos.h>
using namespace Vectormath::Aos;
enum FeatureType { F, E, V };
//----------------------------------------------------------------------------
// Box
//----------------------------------------------------------------------------
///The Box is an internal class used by the boxBoxDistance calculation.
class Box
{
public:
Vector3 half;
inline Box()
{}
inline Box(PE_REF(Vector3) half_);
inline Box(float hx, float hy, float hz);
inline void Set(PE_REF(Vector3) half_);
inline void Set(float hx, float hy, float hz);
inline Vector3 GetAABB(const Matrix3& rotation) const;
};
inline
Box::Box(PE_REF(Vector3) half_)
{
Set(half_);
}
inline
Box::Box(float hx, float hy, float hz)
{
Set(hx, hy, hz);
}
inline
void
Box::Set(PE_REF(Vector3) half_)
{
half = half_;
}
inline
void
Box::Set(float hx, float hy, float hz)
{
half = Vector3(hx, hy, hz);
}
inline
Vector3
Box::GetAABB(const Matrix3& rotation) const
{
return absPerElem(rotation) * half;
}
//-------------------------------------------------------------------------------------------------
// BoxPoint
//-------------------------------------------------------------------------------------------------
///The BoxPoint class is an internally used class to contain feature information for boxBoxDistance calculation.
class BoxPoint
{
public:
BoxPoint() : localPoint(0.0f) {}
Point3 localPoint;
FeatureType featureType;
int featureIdx;
inline void setVertexFeature(int plusX, int plusY, int plusZ);
inline void setEdgeFeature(int dim0, int plus0, int dim1, int plus1);
inline void setFaceFeature(int dim, int plus);
inline void getVertexFeature(int & plusX, int & plusY, int & plusZ) const;
inline void getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const;
inline void getFaceFeature(int & dim, int & plus) const;
};
inline
void
BoxPoint::setVertexFeature(int plusX, int plusY, int plusZ)
{
featureType = V;
featureIdx = plusX << 2 | plusY << 1 | plusZ;
}
inline
void
BoxPoint::setEdgeFeature(int dim0, int plus0, int dim1, int plus1)
{
featureType = E;
if (dim0 > dim1) {
featureIdx = plus1 << 5 | dim1 << 3 | plus0 << 2 | dim0;
} else {
featureIdx = plus0 << 5 | dim0 << 3 | plus1 << 2 | dim1;
}
}
inline
void
BoxPoint::setFaceFeature(int dim, int plus)
{
featureType = F;
featureIdx = plus << 2 | dim;
}
inline
void
BoxPoint::getVertexFeature(int & plusX, int & plusY, int & plusZ) const
{
plusX = featureIdx >> 2;
plusY = featureIdx >> 1 & 1;
plusZ = featureIdx & 1;
}
inline
void
BoxPoint::getEdgeFeature(int & dim0, int & plus0, int & dim1, int & plus1) const
{
plus0 = featureIdx >> 5;
dim0 = featureIdx >> 3 & 3;
plus1 = featureIdx >> 2 & 1;
dim1 = featureIdx & 3;
}
inline
void
BoxPoint::getFaceFeature(int & dim, int & plus) const
{
plus = featureIdx >> 2;
dim = featureIdx & 3;
}
#endif /* __BOX_H__ */

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
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __SPU_COLLISION_SHAPES_H
#define __SPU_COLLISION_SHAPES_H
#include "../SpuDoubleBuffer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#define MAX_NUM_SPU_CONVEX_POINTS 128
struct SpuConvexPolyhedronVertexData
{
void* gSpuConvexShapePtr;
btVector3* gConvexPoints;
int gNumConvexPoints;
ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
};
#define MAX_SHAPE_SIZE 256
struct CollisionShape_LocalStoreMemory
{
ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
};
struct CompoundShape_LocalStoreMemory
{
// Compound data
#define MAX_SPU_COMPOUND_SUBSHAPES 16
ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
};
struct bvhMeshShape_LocalStoreMemory
{
//ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh);
ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
btOptimizedBvh* getOptimizedBvh()
{
return (btOptimizedBvh*) gOptimizedBvh;
}
ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage);
btTriangleIndexVertexArray* gTriangleMeshInterfacePtr;
///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment
ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh);
#define MAX_SPU_SUBTREE_HEADERS 32
//1024
ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]);
};
btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform);
void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag);
void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag);
int getShapeTypeSize(int shapeType);
void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
#define USE_BRANCHFREE_TEST 1
#ifdef USE_BRANCHFREE_TEST
SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
#if defined(__CELLOS_LV2__) && defined (__SPU__)
vec_ushort8 vecMin = {aabbMin1[0],aabbMin2[0],aabbMin1[2],aabbMin2[2],aabbMin1[1],aabbMin2[1],0,0};
vec_ushort8 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0};
vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax);
return spu_extract(spu_gather(isGt),0)==0;
#else
return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0);
#endif
}
#else
SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
unsigned int overlap = 1;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
return overlap;
}
#endif
void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
#endif
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __SPU_COLLISION_SHAPES_H
#define __SPU_COLLISION_SHAPES_H
#include "../SpuDoubleBuffer.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"
#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#define MAX_NUM_SPU_CONVEX_POINTS 128
struct SpuConvexPolyhedronVertexData
{
void* gSpuConvexShapePtr;
btVector3* gConvexPoints;
int gNumConvexPoints;
ATTRIBUTE_ALIGNED16(btVector3 g_convexPointBuffer[MAX_NUM_SPU_CONVEX_POINTS]);
};
#define MAX_SHAPE_SIZE 256
struct CollisionShape_LocalStoreMemory
{
ATTRIBUTE_ALIGNED16(char collisionShape[MAX_SHAPE_SIZE]);
};
struct CompoundShape_LocalStoreMemory
{
// Compound data
#define MAX_SPU_COMPOUND_SUBSHAPES 16
ATTRIBUTE_ALIGNED16(btCompoundShapeChild gSubshapes[MAX_SPU_COMPOUND_SUBSHAPES]);
ATTRIBUTE_ALIGNED16(char gSubshapeShape[MAX_SPU_COMPOUND_SUBSHAPES][MAX_SHAPE_SIZE]);
};
struct bvhMeshShape_LocalStoreMemory
{
//ATTRIBUTE_ALIGNED16(btOptimizedBvh gOptimizedBvh);
ATTRIBUTE_ALIGNED16(char gOptimizedBvh[sizeof(btOptimizedBvh)+16]);
btOptimizedBvh* getOptimizedBvh()
{
return (btOptimizedBvh*) gOptimizedBvh;
}
ATTRIBUTE_ALIGNED16(btTriangleIndexVertexArray gTriangleMeshInterfaceStorage);
btTriangleIndexVertexArray* gTriangleMeshInterfacePtr;
///only a single mesh part for now, we can add support for multiple parts, but quantized trees don't support this at the moment
ATTRIBUTE_ALIGNED16(btIndexedMesh gIndexMesh);
#define MAX_SPU_SUBTREE_HEADERS 32
//1024
ATTRIBUTE_ALIGNED16(btBvhSubtreeInfo gSubtreeHeaders[MAX_SPU_SUBTREE_HEADERS]);
ATTRIBUTE_ALIGNED16(btQuantizedBvhNode gSubtreeNodes[MAX_SUBTREE_SIZE_IN_BYTES/sizeof(btQuantizedBvhNode)]);
};
btVector3 localGetSupportingVertexWithoutMargin(int shapeType, void* shape, const btVector3& localDir,struct SpuConvexPolyhedronVertexData* convexVertexData);//, int *featureIndex)
void computeAabb (btVector3& aabbMin, btVector3& aabbMax, btConvexInternalShape* convexShape, ppu_address_t convexShapePtr, int shapeType, const btTransform& xform);
void dmaBvhShapeData (bvhMeshShape_LocalStoreMemory* bvhMeshShape, btBvhTriangleMeshShape* triMeshShape);
void dmaBvhIndexedMesh (btIndexedMesh* IndexMesh, IndexedMeshArray& indexArray, int index, uint32_t dmaTag);
void dmaBvhSubTreeHeaders (btBvhSubtreeInfo* subTreeHeaders, ppu_address_t subTreePtr, int batchSize, uint32_t dmaTag);
void dmaBvhSubTreeNodes (btQuantizedBvhNode* nodes, const btBvhSubtreeInfo& subtree, QuantizedNodeArray& nodeArray, int dmaTag);
int getShapeTypeSize(int shapeType);
void dmaConvexVertexData (SpuConvexPolyhedronVertexData* convexVertexData, btConvexHullShape* convexShapeSPU);
void dmaCollisionShape (void* collisionShapeLocation, ppu_address_t collisionShapePtr, uint32_t dmaTag, int shapeType);
void dmaCompoundShapeInfo (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
void dmaCompoundSubShapes (CompoundShape_LocalStoreMemory* compoundShapeLocation, btCompoundShape* spuCompoundShape, uint32_t dmaTag);
#define USE_BRANCHFREE_TEST 1
#ifdef USE_BRANCHFREE_TEST
SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
#if defined(__CELLOS_LV2__) && defined (__SPU__)
vec_ushort8 vecMin = {aabbMin1[0],aabbMin2[0],aabbMin1[2],aabbMin2[2],aabbMin1[1],aabbMin2[1],0,0};
vec_ushort8 vecMax = {aabbMax2[0],aabbMax1[0],aabbMax2[2],aabbMax1[2],aabbMax2[1],aabbMax1[1],0,0};
vec_ushort8 isGt = spu_cmpgt(vecMin,vecMax);
return spu_extract(spu_gather(isGt),0)==0;
#else
return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0);
#endif
}
#else
SIMD_FORCE_INLINE unsigned int spuTestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
unsigned int overlap = 1;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? 0 : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? 0 : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? 0 : overlap;
return overlap;
}
#endif
void spuWalkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,const btQuantizedBvhNode* rootNode,int startNodeIndex,int endNodeIndex);
#endif

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

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

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.
All rights reserved.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BOXBOXDISTANCE_H__
#define __BOXBOXDISTANCE_H__
#include "Box.h"
using namespace Vectormath::Aos;
//---------------------------------------------------------------------------
// boxBoxDistance:
//
// description:
// this computes info that can be used for the collision response of two boxes. when the boxes
// do not overlap, the points are set to the closest points of the boxes, and a positive
// distance between them is returned. if the boxes do overlap, a negative distance is returned
// and the points are set to two points that would touch after the boxes are translated apart.
// the contact normal gives the direction to repel or separate the boxes when they touch or
// overlap (it's being approximated here as one of the 15 "separating axis" directions).
//
// returns:
// positive or negative distance between two boxes.
//
// args:
// Vector3& normal: set to a unit contact normal pointing from box A to box B.
//
// BoxPoint& boxPointA, BoxPoint& boxPointB:
// set to a closest point or point of penetration on each box.
//
// Box boxA, Box boxB:
// boxes, represented as 3 half-widths
//
// const Transform3& transformA, const Transform3& transformB:
// box transformations, in world coordinates
//
// float distanceThreshold:
// the algorithm will exit early if it finds that the boxes are more distant than this
// threshold, and not compute a contact normal or points. if this distance returned
// exceeds the threshold, all the other output data may not have been computed. by
// default, this is set to MAX_FLOAT so it will have no effect.
//
//---------------------------------------------------------------------------
float
boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB,
const Transform3 & transformB,
float distanceThreshold = FLT_MAX );
#endif /* __BOXBOXDISTANCE_H__ */
/*
Copyright (C) 2006, 2008 Sony Computer Entertainment Inc.
All rights reserved.
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __BOXBOXDISTANCE_H__
#define __BOXBOXDISTANCE_H__
#include "Box.h"
using namespace Vectormath::Aos;
//---------------------------------------------------------------------------
// boxBoxDistance:
//
// description:
// this computes info that can be used for the collision response of two boxes. when the boxes
// do not overlap, the points are set to the closest points of the boxes, and a positive
// distance between them is returned. if the boxes do overlap, a negative distance is returned
// and the points are set to two points that would touch after the boxes are translated apart.
// the contact normal gives the direction to repel or separate the boxes when they touch or
// overlap (it's being approximated here as one of the 15 "separating axis" directions).
//
// returns:
// positive or negative distance between two boxes.
//
// args:
// Vector3& normal: set to a unit contact normal pointing from box A to box B.
//
// BoxPoint& boxPointA, BoxPoint& boxPointB:
// set to a closest point or point of penetration on each box.
//
// Box boxA, Box boxB:
// boxes, represented as 3 half-widths
//
// const Transform3& transformA, const Transform3& transformB:
// box transformations, in world coordinates
//
// float distanceThreshold:
// the algorithm will exit early if it finds that the boxes are more distant than this
// threshold, and not compute a contact normal or points. if this distance returned
// exceeds the threshold, all the other output data may not have been computed. by
// default, this is set to MAX_FLOAT so it will have no effect.
//
//---------------------------------------------------------------------------
float
boxBoxDistance(Vector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,
PE_REF(Box) boxA, const Transform3 & transformA, PE_REF(Box) boxB,
const Transform3 & transformB,
float distanceThreshold = FLT_MAX );
#endif /* __BOXBOXDISTANCE_H__ */

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

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

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

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

@@ -1,60 +1,60 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_SUBSIMPLEX_RAY_CAST_H
#define SPU_SUBSIMPLEX_RAY_CAST_H
#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h"
#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
#include "SpuRaycastTask.h"
class btConvexShape;
struct SpuCastResult
{
float m_fraction;
btVector3 m_normal;
};
/// btSubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
/// GJK based Ray Cast, optimized version
/// Objects should not start in overlap, otherwise results are not defined.
class SpuSubsimplexRayCast
{
SpuVoronoiSimplexSolver* m_simplexSolver;
void* m_shapeB;
SpuConvexPolyhedronVertexData* m_convexDataB;
int m_shapeTypeB;
float m_marginB;
public:
SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
SpuVoronoiSimplexSolver* simplexSolver);
//virtual ~btSubsimplexConvexCast();
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
bool calcTimeOfImpact(const btTransform& fromRay,
const btTransform& toRay,
const btTransform& fromB,
const btTransform& toB,
SpuCastResult& result);
};
#endif //SUBSIMPLEX_RAY_CAST_H
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_SUBSIMPLEX_RAY_CAST_H
#define SPU_SUBSIMPLEX_RAY_CAST_H
#include "../SpuNarrowPhaseCollisionTask/SpuVoronoiSimplexSolver.h"
#include "../SpuNarrowPhaseCollisionTask/SpuCollisionShapes.h"
#include "SpuRaycastTask.h"
class btConvexShape;
struct SpuCastResult
{
float m_fraction;
btVector3 m_normal;
};
/// btSubsimplexConvexCast implements Gino van den Bergens' paper
///"Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
/// GJK based Ray Cast, optimized version
/// Objects should not start in overlap, otherwise results are not defined.
class SpuSubsimplexRayCast
{
SpuVoronoiSimplexSolver* m_simplexSolver;
void* m_shapeB;
SpuConvexPolyhedronVertexData* m_convexDataB;
int m_shapeTypeB;
float m_marginB;
public:
SpuSubsimplexRayCast (void* shapeB, SpuConvexPolyhedronVertexData* convexDataB, int shapeTypeB, float marginB,
SpuVoronoiSimplexSolver* simplexSolver);
//virtual ~btSubsimplexConvexCast();
///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
///Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using btGjkPairDetector.
bool calcTimeOfImpact(const btTransform& fromRay,
const btTransform& toRay,
const btTransform& fromB,
const btTransform& toB,
SpuCastResult& result);
};
#endif //SUBSIMPLEX_RAY_CAST_H

378
src/BulletMultiThreaded/SpuRaycastTaskProcess.cpp
View File

@@ -1,189 +1,189 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuRaycastTaskProcess.h"
SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks)
:m_threadInterface(threadInterface),
m_maxNumOutstandingTasks(maxNumOutstandingTasks)
{
m_workUnitTaskBuffers = (unsigned char *)0;
m_taskBusy.resize(m_maxNumOutstandingTasks);
m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks);
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
m_taskBusy[i] = false;
}
m_numBusyTasks = 0;
m_currentTask = 0;
m_currentWorkUnitInTask = 0;
m_threadInterface->startSPU();
//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
//printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
}
SpuRaycastTaskProcess::~SpuRaycastTaskProcess()
{
if (m_workUnitTaskBuffers != 0)
{
btAlignedFree(m_workUnitTaskBuffers);
m_workUnitTaskBuffers = 0;
}
m_threadInterface->stopSPU();
}
void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers)
{
m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers;
m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers;
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
m_taskBusy[i] = false;
}
m_numBusyTasks = 0;
m_currentTask = 0;
m_currentWorkUnitInTask = 0;
#ifdef DEBUG_SpuRaycastTaskProcess
m_initialized = true;
#endif
}
void SpuRaycastTaskProcess::issueTask2()
{
m_taskBusy[m_currentTask] = true;
m_numBusyTasks++;
SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask];
taskDesc.taskId = m_currentTask;
m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask);
//printf("send thread requested for task %d\n", m_currentTask);
// if all tasks busy, wait for spu event to clear the task.
if (m_numBusyTasks >= m_maxNumOutstandingTasks)
{
unsigned int taskId;
unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++)
{
if (m_taskBusy[i])
{
taskId = i;
break;
}
}
m_threadInterface->waitForResponse(&taskId, &outputSize);
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
m_taskBusy[taskId] = false;
m_numBusyTasks--;
} else {
//printf("Sent request, not enough busy tasks\n");
}
}
void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit)
{
m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit;
m_currentWorkUnitInTask++;
if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK)
{
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
//printf("Task buffer full, issuing\n");
issueTask2 ();
//printf("Returned from issueTask2()\n");
m_currentWorkUnitInTask = 0;
// find new task buffer
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
if (!m_taskBusy[i])
{
m_currentTask = i;
//init the task data
break;
}
}
//printf("next task = %d\n", m_currentTask);
}
}
void
SpuRaycastTaskProcess::flush2()
{
#ifdef DEBUG_SPU_TASK_SCHEDULING
printf("\nSpuRaycastTaskProcess::flush()\n");
#endif //DEBUG_SPU_TASK_SCHEDULING
// if there's a partially filled task buffer, submit that task
//printf("Flushing... %d remaining\n", m_currentWorkUnitInTask);
if (m_currentWorkUnitInTask > 0)
{
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
issueTask2();
m_currentWorkUnitInTask = 0;
}
// all tasks are issued, wait for all tasks to be complete
while(m_numBusyTasks > 0)
{
// Consolidating SPU code
unsigned int taskId;
unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++)
{
if (m_taskBusy[i])
{
taskId = i;
break;
}
}
//printf("Busy tasks... %d\n", m_numBusyTasks);
{
// SPURS support.
m_threadInterface->waitForResponse(&taskId, &outputSize);
}
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
//postProcess(taskId, outputSize);
m_taskBusy[taskId] = false;
m_numBusyTasks--;
}
}
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuRaycastTaskProcess.h"
SpuRaycastTaskProcess::SpuRaycastTaskProcess(class btThreadSupportInterface* threadInterface, int maxNumOutstandingTasks)
:m_threadInterface(threadInterface),
m_maxNumOutstandingTasks(maxNumOutstandingTasks)
{
m_workUnitTaskBuffers = (unsigned char *)0;
m_taskBusy.resize(m_maxNumOutstandingTasks);
m_spuRaycastTaskDesc.resize(m_maxNumOutstandingTasks);
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
m_taskBusy[i] = false;
}
m_numBusyTasks = 0;
m_currentTask = 0;
m_currentWorkUnitInTask = 0;
m_threadInterface->startSPU();
//printf("sizeof vec_float4: %d\n", sizeof(vec_float4));
//printf("sizeof SpuGatherAndProcessWorkUnitInput: %d\n", sizeof(SpuGatherAndProcessWorkUnitInput));
}
SpuRaycastTaskProcess::~SpuRaycastTaskProcess()
{
if (m_workUnitTaskBuffers != 0)
{
btAlignedFree(m_workUnitTaskBuffers);
m_workUnitTaskBuffers = 0;
}
m_threadInterface->stopSPU();
}
void SpuRaycastTaskProcess::initialize2(void* spuCollisionObjectsWrappers, int numSpuCollisionObjectWrappers)
{
m_spuCollisionObjectWrappers = spuCollisionObjectsWrappers;
m_numSpuCollisionObjectWrappers = numSpuCollisionObjectWrappers;
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
m_taskBusy[i] = false;
}
m_numBusyTasks = 0;
m_currentTask = 0;
m_currentWorkUnitInTask = 0;
#ifdef DEBUG_SpuRaycastTaskProcess
m_initialized = true;
#endif
}
void SpuRaycastTaskProcess::issueTask2()
{
m_taskBusy[m_currentTask] = true;
m_numBusyTasks++;
SpuRaycastTaskDesc& taskDesc = m_spuRaycastTaskDesc[m_currentTask];
taskDesc.taskId = m_currentTask;
m_threadInterface->sendRequest(1, (ppu_address_t) &taskDesc,m_currentTask);
//printf("send thread requested for task %d\n", m_currentTask);
// if all tasks busy, wait for spu event to clear the task.
if (m_numBusyTasks >= m_maxNumOutstandingTasks)
{
unsigned int taskId;
unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++)
{
if (m_taskBusy[i])
{
taskId = i;
break;
}
}
m_threadInterface->waitForResponse(&taskId, &outputSize);
//printf("PPU: after issue, received event: %u %d\n", taskId, outputSize);
m_taskBusy[taskId] = false;
m_numBusyTasks--;
} else {
//printf("Sent request, not enough busy tasks\n");
}
}
void SpuRaycastTaskProcess::addWorkToTask(SpuRaycastTaskWorkUnit& workunit)
{
m_spuRaycastTaskDesc[m_currentTask].workUnits[m_currentWorkUnitInTask] = workunit;
m_currentWorkUnitInTask++;
if (m_currentWorkUnitInTask == SPU_RAYCAST_WORK_UNITS_PER_TASK)
{
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
//printf("Task buffer full, issuing\n");
issueTask2 ();
//printf("Returned from issueTask2()\n");
m_currentWorkUnitInTask = 0;
// find new task buffer
for (int i = 0; i < m_maxNumOutstandingTasks; i++)
{
if (!m_taskBusy[i])
{
m_currentTask = i;
//init the task data
break;
}
}
//printf("next task = %d\n", m_currentTask);
}
}
void
SpuRaycastTaskProcess::flush2()
{
#ifdef DEBUG_SPU_TASK_SCHEDULING
printf("\nSpuRaycastTaskProcess::flush()\n");
#endif //DEBUG_SPU_TASK_SCHEDULING
// if there's a partially filled task buffer, submit that task
//printf("Flushing... %d remaining\n", m_currentWorkUnitInTask);
if (m_currentWorkUnitInTask > 0)
{
m_spuRaycastTaskDesc[m_currentTask].numWorkUnits = m_currentWorkUnitInTask;
m_spuRaycastTaskDesc[m_currentTask].numSpuCollisionObjectWrappers = m_numSpuCollisionObjectWrappers;
m_spuRaycastTaskDesc[m_currentTask].spuCollisionObjectsWrappers = m_spuCollisionObjectWrappers;
issueTask2();
m_currentWorkUnitInTask = 0;
}
// all tasks are issued, wait for all tasks to be complete
while(m_numBusyTasks > 0)
{
// Consolidating SPU code
unsigned int taskId;
unsigned int outputSize;
for (int i=0;i<m_maxNumOutstandingTasks;i++)
{
if (m_taskBusy[i])
{
taskId = i;
break;
}
}
//printf("Busy tasks... %d\n", m_numBusyTasks);
{
// SPURS support.
m_threadInterface->waitForResponse(&taskId, &outputSize);
}
//printf("PPU: flushing, received event: %u %d\n", taskId, outputSize);
//postProcess(taskId, outputSize);
m_taskBusy[taskId] = false;
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
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuSampleTask.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "../PlatformDefinitions.h"
#include "../SpuFakeDma.h"
#include "LinearMath/btMinMax.h"
#ifdef __SPU__
#include <spu_printf.h>
#else
#include <stdio.h>
#define spu_printf printf
#endif
#define MAX_NUM_BODIES 8192
struct SampleTask_LocalStoreMemory
{
ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]);
ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]);
};
//-- MAIN METHOD
void processSampleTask(void* userPtr, void* lsMemory)
{
// BT_PROFILE("processSampleTask");
SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory;
SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr;
SpuSampleTaskDesc& taskDesc = *taskDescPtr;
switch (taskDesc.m_sampleCommand)
{
case CMD_SAMPLE_INTEGRATE_BODIES:
{
btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue;
if (batchSize>MAX_NUM_BODIES)
{
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break;
}
int dmaArraySize = batchSize*sizeof(void*);
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16)
{
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
} else
{
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
}
for ( int i=0;i<batchSize;i++)
{
///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body)
{
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans);
void* ptr = (void*)localPtr;
// spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress);
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
break;
}
case CMD_SAMPLE_PREDICT_MOTION_BODIES:
{
btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue;
int dmaArraySize = batchSize*sizeof(void*);
if (batchSize>MAX_NUM_BODIES)
{
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break;
}
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16)
{
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
} else
{
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
}
for ( int i=0;i<batchSize;i++)
{
///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body)
{
if (!body->isStaticOrKinematicObject())
{
if (body->isActive())
{
body->integrateVelocities( timeStep);
//damping
body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
void* ptr = (void*)localPtr;
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
}
break;
}
default:
{
}
};
}
#if defined(__CELLOS_LV2__) || defined (LIBSPE2)
ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory);
void* createSampleLocalStoreMemory()
{
return &gLocalStoreMemory;
}
#else
void* createSampleLocalStoreMemory()
{
return new SampleTask_LocalStoreMemory;
};
#endif
/*
Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SpuSampleTask.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "../PlatformDefinitions.h"
#include "../SpuFakeDma.h"
#include "LinearMath/btMinMax.h"
#ifdef __SPU__
#include <spu_printf.h>
#else
#include <stdio.h>
#define spu_printf printf
#endif
#define MAX_NUM_BODIES 8192
struct SampleTask_LocalStoreMemory
{
ATTRIBUTE_ALIGNED16(char gLocalRigidBody [sizeof(btRigidBody)+16]);
ATTRIBUTE_ALIGNED16(void* gPointerArray[MAX_NUM_BODIES]);
};
//-- MAIN METHOD
void processSampleTask(void* userPtr, void* lsMemory)
{
// BT_PROFILE("processSampleTask");
SampleTask_LocalStoreMemory* localMemory = (SampleTask_LocalStoreMemory*)lsMemory;
SpuSampleTaskDesc* taskDescPtr = (SpuSampleTaskDesc*)userPtr;
SpuSampleTaskDesc& taskDesc = *taskDescPtr;
switch (taskDesc.m_sampleCommand)
{
case CMD_SAMPLE_INTEGRATE_BODIES:
{
btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue;
if (batchSize>MAX_NUM_BODIES)
{
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break;
}
int dmaArraySize = batchSize*sizeof(void*);
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16)
{
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
} else
{
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
}
for ( int i=0;i<batchSize;i++)
{
///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body)
{
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
body->proceedToTransform( predictedTrans);
void* ptr = (void*)localPtr;
// spu_printf("cellDmaLargePut from %llx to LS %llx\n",ptr,ppuRigidBodyAddress);
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
break;
}
case CMD_SAMPLE_PREDICT_MOTION_BODIES:
{
btTransform predictedTrans;
btCollisionObject** eaPtr = (btCollisionObject**)taskDesc.m_mainMemoryPtr;
int batchSize = taskDesc.m_sampleValue;
int dmaArraySize = batchSize*sizeof(void*);
if (batchSize>MAX_NUM_BODIES)
{
spu_printf("SPU Error: exceed number of bodies, see MAX_NUM_BODIES in SpuSampleTask.cpp\n");
break;
}
uint64_t ppuArrayAddress = reinterpret_cast<uint64_t>(eaPtr);
// spu_printf("array location is at %llx, batchSize = %d, DMA size = %d\n",ppuArrayAddress,batchSize,dmaArraySize);
if (dmaArraySize>=16)
{
cellDmaLargeGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
} else
{
stallingUnalignedDmaSmallGet((void*)&localMemory->gPointerArray[0], ppuArrayAddress , dmaArraySize);
}
for ( int i=0;i<batchSize;i++)
{
///DMA rigid body
void* localPtr = &localMemory->gLocalRigidBody[0];
void* shortAdd = localMemory->gPointerArray[i];
uint64_t ppuRigidBodyAddress = reinterpret_cast<uint64_t>(shortAdd);
// spu_printf("cellDmaGet at CMD_SAMPLE_INTEGRATE_BODIES from %llx to %llx\n",ppuRigidBodyAddress,localPtr);
int dmaBodySize = sizeof(btRigidBody);
cellDmaGet((void*)localPtr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
float timeStep = 1.f/60.f;
btRigidBody* body = (btRigidBody*) localPtr;//btRigidBody::upcast(colObj);
if (body)
{
if (!body->isStaticOrKinematicObject())
{
if (body->isActive())
{
body->integrateVelocities( timeStep);
//damping
body->applyDamping(timeStep);
body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
void* ptr = (void*)localPtr;
cellDmaLargePut(ptr, ppuRigidBodyAddress , dmaBodySize, DMA_TAG(1), 0, 0);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
}
break;
}
default:
{
}
};
}
#if defined(__CELLOS_LV2__) || defined (LIBSPE2)
ATTRIBUTE_ALIGNED16(SampleTask_LocalStoreMemory gLocalStoreMemory);
void* createSampleLocalStoreMemory()
{
return &gLocalStoreMemory;
}
#else
void* createSampleLocalStoreMemory()
{
return new SampleTask_LocalStoreMemory;
};
#endif

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

@@ -1,54 +1,54 @@
/*
Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_SAMPLE_TASK_H
#define SPU_SAMPLE_TASK_H
#include "../PlatformDefinitions.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedAllocator.h"
enum
{
CMD_SAMPLE_INTEGRATE_BODIES = 1,
CMD_SAMPLE_PREDICT_MOTION_BODIES
};
ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc
{
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_sampleCommand;
uint32_t m_taskId;
uint64_t m_mainMemoryPtr;
int m_sampleValue;
};
void processSampleTask(void* userPtr, void* lsMemory);
void* createSampleLocalStoreMemory();
#endif //SPU_SAMPLE_TASK_H
/*
Bullet Continuous Collision Detection and Physics Library, Copyright (c) 2007 Erwin Coumans
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPU_SAMPLE_TASK_H
#define SPU_SAMPLE_TASK_H
#include "../PlatformDefinitions.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btAlignedAllocator.h"
enum
{
CMD_SAMPLE_INTEGRATE_BODIES = 1,
CMD_SAMPLE_PREDICT_MOTION_BODIES
};
ATTRIBUTE_ALIGNED16(struct) SpuSampleTaskDesc
{
BT_DECLARE_ALIGNED_ALLOCATOR();
uint32_t m_sampleCommand;
uint32_t m_taskId;
uint64_t m_mainMemoryPtr;
int m_sampleValue;
};
void processSampleTask(void* userPtr, void* lsMemory);
void* createSampleLocalStoreMemory();
#endif //SPU_SAMPLE_TASK_H

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

250
src/BulletMultiThreaded/Win32ThreadSupport.h
View File

@@ -1,125 +1,125 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h"
#ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h
#ifndef WIN32_THREAD_SUPPORT_H
#define WIN32_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h"
typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*Win32lsMemorySetupFunc)();
///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class Win32ThreadSupport : public btThreadSupportInterface
{
public:
///placeholder, until libspe2 support is there
struct btSpuStatus
{
uint32_t m_taskId;
uint32_t m_commandId;
uint32_t m_status;
Win32ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc
void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_eventStartHandle;
char m_eventStartHandleName[32];
void* m_eventCompletetHandle;
char m_eventCompletetHandleName[32];
};
private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles;
public:
///Setup and initialize SPU/CELL/Libspe2
struct Win32ThreadConstructionInfo
{
Win32ThreadConstructionInfo(char* uniqueName,
Win32ThreadFunc userThreadFunc,
Win32lsMemorySetupFunc lsMemoryFunc,
int numThreads=1,
int threadStackSize=65535
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_numThreads(numThreads),
m_threadStackSize(threadStackSize)
{
}
char* m_uniqueName;
Win32ThreadFunc m_userThreadFunc;
Win32lsMemorySetupFunc m_lsMemoryFunc;
int m_numThreads;
int m_threadStackSize;
};
Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo);
///cleanup/shutdown Libspe2
virtual ~Win32ThreadSupport();
void startThreads(const Win32ThreadConstructionInfo& threadInfo);
///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU();
virtual void setNumTasks(int numTasks)
{
}
};
#endif //WIN32_THREAD_SUPPORT_H
#endif //USE_WIN32_THREADING
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h"
#ifdef USE_WIN32_THREADING //platform specific defines are defined in PlatformDefinitions.h
#ifndef WIN32_THREAD_SUPPORT_H
#define WIN32_THREAD_SUPPORT_H
#include "LinearMath/btAlignedObjectArray.h"
#include "btThreadSupportInterface.h"
typedef void (*Win32ThreadFunc)(void* userPtr,void* lsMemory);
typedef void* (*Win32lsMemorySetupFunc)();
///Win32ThreadSupport helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class Win32ThreadSupport : public btThreadSupportInterface
{
public:
///placeholder, until libspe2 support is there
struct btSpuStatus
{
uint32_t m_taskId;
uint32_t m_commandId;
uint32_t m_status;
Win32ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
void* m_lsMemory; //initialized using Win32LocalStoreMemorySetupFunc
void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_eventStartHandle;
char m_eventStartHandleName[32];
void* m_eventCompletetHandle;
char m_eventCompletetHandleName[32];
};
private:
btAlignedObjectArray<btSpuStatus> m_activeSpuStatus;
btAlignedObjectArray<void*> m_completeHandles;
public:
///Setup and initialize SPU/CELL/Libspe2
struct Win32ThreadConstructionInfo
{
Win32ThreadConstructionInfo(char* uniqueName,
Win32ThreadFunc userThreadFunc,
Win32lsMemorySetupFunc lsMemoryFunc,
int numThreads=1,
int threadStackSize=65535
)
:m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_lsMemoryFunc(lsMemoryFunc),
m_numThreads(numThreads),
m_threadStackSize(threadStackSize)
{
}
char* m_uniqueName;
Win32ThreadFunc m_userThreadFunc;
Win32lsMemorySetupFunc m_lsMemoryFunc;
int m_numThreads;
int m_threadStackSize;
};
Win32ThreadSupport(const Win32ThreadConstructionInfo& threadConstructionInfo);
///cleanup/shutdown Libspe2
virtual ~Win32ThreadSupport();
void startThreads(const Win32ThreadConstructionInfo& threadInfo);
///send messages to SPUs
virtual void sendRequest(uint32_t uiCommand, ppu_address_t uiArgument0, uint32_t uiArgument1);
///check for messages from SPUs
virtual void waitForResponse(unsigned int *puiArgument0, unsigned int *puiArgument1);
///start the spus (can be called at the beginning of each frame, to make sure that the right SPU program is loaded)
virtual void startSPU();
///tell the task scheduler we are done with the SPU tasks
virtual void stopSPU();
virtual void setNumTasks(int numTasks)
{
}
};
#endif //WIN32_THREAD_SUPPORT_H
#endif //USE_WIN32_THREADING

146
src/BulletMultiThreaded/vectormath2bullet.h
View File

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

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

306
src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h
View File

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

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