diff options
164 files changed, 21796 insertions, 0 deletions
diff --git a/extern/bullet2/readme.txt b/extern/bullet2/readme.txt new file mode 100644 index 00000000000..4d1a4c11706 --- /dev/null +++ b/extern/bullet2/readme.txt @@ -0,0 +1,12 @@ + +*** These files in extern/bullet2 are NOT part of the Blender build yet *** + +This is the new refactored version of Bullet physics library version 2.x + +Soon this will replace the old Bullet version in extern/bullet. +First the integration in Blender Game Engine needs to be updated. +Once that is done all build systems can be updated to use/build extern/bullet2 files. + +Questions? mail blender at erwincoumans.com, or check the bf-blender mailing list. +Thanks, +Erwin diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp new file mode 100644 index 00000000000..b05285ca727 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -0,0 +1,499 @@ + +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + + +// +// btAxisSweep3 +// +// Copyright (c) 2006 Simon Hobbs +// +// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +// +// 2. Altered source versions must be plainly marked as 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 "btAxisSweep3.h" + +#include <assert.h> + +btBroadphaseProxy* btAxisSweep3::createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) +{ + unsigned short handleId = addHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask); + + Handle* handle = getHandle(handleId); + + return handle; +} + +void btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy) +{ + Handle* handle = static_cast<Handle*>(proxy); + removeHandle(handle->m_handleId); +} + +void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax) +{ + Handle* handle = static_cast<Handle*>(proxy); + updateHandle(handle->m_handleId,aabbMin,aabbMax); +} + + + + + + +btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles) +:btOverlappingPairCache() +{ + //assert(bounds.HasVolume()); + + // 1 handle is reserved as sentinel + assert(maxHandles > 1 && maxHandles < 32767); + + // init bounds + m_worldAabbMin = worldAabbMin; + m_worldAabbMax = worldAabbMax; + + btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; + + m_quantize = btVector3(65535.0f,65535.0f,65535.0f) / aabbSize; + + // allocate handles buffer and put all handles on free list + m_pHandles = new Handle[maxHandles]; + m_maxHandles = maxHandles; + m_numHandles = 0; + + // handle 0 is reserved as the null index, and is also used as the sentinel + m_firstFreeHandle = 1; + { + for (int i = m_firstFreeHandle; i < maxHandles; i++) + m_pHandles[i].SetNextFree(i + 1); + m_pHandles[maxHandles - 1].SetNextFree(0); + } + + { + // allocate edge buffers + for (int i = 0; i < 3; i++) + m_pEdges[i] = new Edge[maxHandles * 2]; + } + //removed overlap management + + // make boundary sentinels + + m_pHandles[0].m_clientObject = 0; + + for (int axis = 0; axis < 3; axis++) + { + m_pHandles[0].m_minEdges[axis] = 0; + m_pHandles[0].m_maxEdges[axis] = 1; + + m_pEdges[axis][0].m_pos = 0; + m_pEdges[axis][0].m_handle = 0; + m_pEdges[axis][1].m_pos = 0xffff; + m_pEdges[axis][1].m_handle = 0; + } +} + +btAxisSweep3::~btAxisSweep3() +{ + + for (int i = 2; i >= 0; i--) + delete[] m_pEdges[i]; + delete[] m_pHandles; +} + +void btAxisSweep3::quantize(unsigned short* out, const btPoint3& point, int isMax) const +{ + btPoint3 clampedPoint(point); + /* + if (isMax) + clampedPoint += btVector3(10,10,10); + else + { + clampedPoint -= btVector3(10,10,10); + } + */ + + + clampedPoint.setMax(m_worldAabbMin); + clampedPoint.setMin(m_worldAabbMax); + + btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; + out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax); + out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax); + out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax); + +} + + + +unsigned short btAxisSweep3::allocHandle() +{ + assert(m_firstFreeHandle); + + unsigned short handle = m_firstFreeHandle; + m_firstFreeHandle = getHandle(handle)->GetNextFree(); + m_numHandles++; + + return handle; +} + +void btAxisSweep3::freeHandle(unsigned short handle) +{ + assert(handle > 0 && handle < m_maxHandles); + + getHandle(handle)->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numHandles--; +} + + + +unsigned short btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask) +{ + // quantize the bounds + unsigned short min[3], max[3]; + quantize(min, aabbMin, 0); + quantize(max, aabbMax, 1); + + // allocate a handle + unsigned short handle = allocHandle(); + assert(handle!= 0xcdcd); + + Handle* pHandle = getHandle(handle); + + pHandle->m_handleId = handle; + //pHandle->m_pOverlaps = 0; + pHandle->m_clientObject = pOwner; + pHandle->m_collisionFilterGroup = collisionFilterGroup; + pHandle->m_collisionFilterMask = collisionFilterMask; + + // compute current limit of edge arrays + int limit = m_numHandles * 2; + + // insert new edges just inside the max boundary edge + for (int axis = 0; axis < 3; axis++) + { + m_pHandles[0].m_maxEdges[axis] += 2; + + m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; + + m_pEdges[axis][limit - 1].m_pos = min[axis]; + m_pEdges[axis][limit - 1].m_handle = handle; + + m_pEdges[axis][limit].m_pos = max[axis]; + m_pEdges[axis][limit].m_handle = handle; + + pHandle->m_minEdges[axis] = limit - 1; + pHandle->m_maxEdges[axis] = limit; + } + + // now sort the new edges to their correct position + sortMinDown(0, pHandle->m_minEdges[0], false); + sortMaxDown(0, pHandle->m_maxEdges[0], false); + sortMinDown(1, pHandle->m_minEdges[1], false); + sortMaxDown(1, pHandle->m_maxEdges[1], false); + sortMinDown(2, pHandle->m_minEdges[2], true); + sortMaxDown(2, pHandle->m_maxEdges[2], true); + + //PrintAxis(1); + + return handle; +} + + +void btAxisSweep3::removeHandle(unsigned short handle) +{ + Handle* pHandle = getHandle(handle); + + //explicitly remove the pairs containing the proxy + //we could do it also in the sortMinUp (passing true) + //todo: compare performance + removeOverlappingPairsContainingProxy(pHandle); + + + // compute current limit of edge arrays + int limit = m_numHandles * 2; + int axis; + + for (axis = 0;axis<3;axis++) + { + Edge* pEdges = m_pEdges[axis]; + int maxEdge= pHandle->m_maxEdges[axis]; + pEdges[maxEdge].m_pos = 0xffff; + int minEdge = pHandle->m_minEdges[axis]; + pEdges[minEdge].m_pos = 0xffff; + } + + // remove the edges by sorting them up to the end of the list + for ( axis = 0; axis < 3; axis++) + { + Edge* pEdges = m_pEdges[axis]; + int max = pHandle->m_maxEdges[axis]; + pEdges[max].m_pos = 0xffff; + + sortMaxUp(axis,max,false); + + int i = pHandle->m_minEdges[axis]; + pEdges[i].m_pos = 0xffff; + + sortMinUp(axis,i,false); + + pEdges[limit-1].m_handle = 0; + pEdges[limit-1].m_pos = 0xffff; + + } + + // free the handle + freeHandle(handle); + + +} + +bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB) +{ + //optimization 1: check the array index (memory address), instead of the m_pos + + for (int axis = 0; axis < 3; axis++) + { + if (axis != ignoreAxis) + { + if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || + pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) + { + return false; + } + } + } + + //optimization 2: only 2 axis need to be tested + + /*for (int axis = 0; axis < 3; axis++) + { + if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos || + m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos) + { + return false; + } + } + */ + + return true; +} + +void btAxisSweep3::updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax) +{ +// assert(bounds.IsFinite()); + //assert(bounds.HasVolume()); + + Handle* pHandle = getHandle(handle); + + // quantize the new bounds + unsigned short min[3], max[3]; + quantize(min, aabbMin, 0); + quantize(max, aabbMax, 1); + + // update changed edges + for (int axis = 0; axis < 3; axis++) + { + unsigned short emin = pHandle->m_minEdges[axis]; + unsigned short emax = pHandle->m_maxEdges[axis]; + + int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; + int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; + + m_pEdges[axis][emin].m_pos = min[axis]; + m_pEdges[axis][emax].m_pos = max[axis]; + + // expand (only adds overlaps) + if (dmin < 0) + sortMinDown(axis, emin); + + if (dmax > 0) + sortMaxUp(axis, emax); + + // shrink (only removes overlaps) + if (dmin > 0) + sortMinUp(axis, emin); + + if (dmax < 0) + sortMaxDown(axis, emax); + } + + //PrintAxis(1); +} + +// sorting a min edge downwards can only ever *add* overlaps +void btAxisSweep3::sortMinDown(int axis, unsigned short edge, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (pPrev->IsMax()) + { + // if previous edge is a maximum check the bounds and add an overlap if necessary + if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev)) + { + addOverlappingPair(pHandleEdge,pHandlePrev); + + //AddOverlap(pEdge->m_handle, pPrev->m_handle); + + } + + // update edge reference in other handle + pHandlePrev->m_maxEdges[axis]++; + } + else + pHandlePrev->m_minEdges[axis]++; + + pHandleEdge->m_minEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } +} + +// sorting a min edge upwards can only ever *remove* overlaps +void btAxisSweep3::sortMinUp(int axis, unsigned short edge, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos > pNext->m_pos) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + if (pNext->IsMax()) + { + // if next edge is maximum remove any overlap between the two handles + if (updateOverlaps) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + btBroadphasePair tmpPair(*handle0,*handle1); + removeOverlappingPair(tmpPair); + + } + + // update edge reference in other handle + pHandleNext->m_maxEdges[axis]--; + } + else + pHandleNext->m_minEdges[axis]--; + + pHandleEdge->m_minEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } +} + +// sorting a max edge downwards can only ever *remove* overlaps +void btAxisSweep3::sortMaxDown(int axis, unsigned short edge, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pPrev = pEdge - 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos < pPrev->m_pos) + { + Handle* pHandlePrev = getHandle(pPrev->m_handle); + + if (!pPrev->IsMax()) + { + // if previous edge was a minimum remove any overlap between the two handles + if (updateOverlaps) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pPrev->m_handle); + btBroadphasePair* pair = findPair(handle0,handle1); + //assert(pair); + + if (pair) + { + removeOverlappingPair(*pair); + } + } + + // update edge reference in other handle + pHandlePrev->m_minEdges[axis]++;; + } + else + pHandlePrev->m_maxEdges[axis]++; + + pHandleEdge->m_maxEdges[axis]--; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pPrev; + *pPrev = swap; + + // decrement + pEdge--; + pPrev--; + } +} + +// sorting a max edge upwards can only ever *add* overlaps +void btAxisSweep3::sortMaxUp(int axis, unsigned short edge, bool updateOverlaps) +{ + Edge* pEdge = m_pEdges[axis] + edge; + Edge* pNext = pEdge + 1; + Handle* pHandleEdge = getHandle(pEdge->m_handle); + + while (pEdge->m_pos > pNext->m_pos) + { + Handle* pHandleNext = getHandle(pNext->m_handle); + + if (!pNext->IsMax()) + { + // if next edge is a minimum check the bounds and add an overlap if necessary + if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext)) + { + Handle* handle0 = getHandle(pEdge->m_handle); + Handle* handle1 = getHandle(pNext->m_handle); + addOverlappingPair(handle0,handle1); + } + + // update edge reference in other handle + pHandleNext->m_minEdges[axis]--; + } + else + pHandleNext->m_maxEdges[axis]--; + + pHandleEdge->m_maxEdges[axis]++; + + // swap the edges + Edge swap = *pEdge; + *pEdge = *pNext; + *pNext = swap; + + // increment + pEdge++; + pNext++; + } +} diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h new file mode 100644 index 00000000000..ebbbe01bbe6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -0,0 +1,115 @@ +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +// +// btAxisSweep3.h +// +// Copyright (c) 2006 Simon Hobbs +// +// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +// +// 2. Altered source versions must be plainly marked as 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 AXIS_SWEEP_3_H +#define AXIS_SWEEP_3_H + +#include "LinearMath/btPoint3.h" +#include "LinearMath/btVector3.h" +#include "btOverlappingPairCache.h" +#include "btBroadphaseProxy.h" + +/// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. +/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats. +/// The testOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos +class btAxisSweep3 : public btOverlappingPairCache +{ + +public: + + + class Edge + { + public: + unsigned short m_pos; // low bit is min/max + unsigned short m_handle; + + unsigned short IsMax() const {return m_pos & 1;} + }; + +public: + class Handle : public btBroadphaseProxy + { + public: + + // indexes into the edge arrays + unsigned short m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 + unsigned short m_handleId; + unsigned short m_pad; + + //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject + + inline void SetNextFree(unsigned short next) {m_minEdges[0] = next;} + inline unsigned short GetNextFree() const {return m_minEdges[0];} + }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry + + +private: + btPoint3 m_worldAabbMin; // overall system bounds + btPoint3 m_worldAabbMax; // overall system bounds + + btVector3 m_quantize; // scaling factor for quantization + + int m_numHandles; // number of active handles + int m_maxHandles; // max number of handles + Handle* m_pHandles; // handles pool + unsigned short m_firstFreeHandle; // free handles list + + Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) + + + // allocation/deallocation + unsigned short allocHandle(); + void freeHandle(unsigned short handle); + + + bool testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB); + + //Overlap* AddOverlap(unsigned short handleA, unsigned short handleB); + //void RemoveOverlap(unsigned short handleA, unsigned short handleB); + + void quantize(unsigned short* out, const btPoint3& point, int isMax) const; + + void sortMinDown(int axis, unsigned short edge, bool updateOverlaps = true); + void sortMinUp(int axis, unsigned short edge, bool updateOverlaps = true); + void sortMaxDown(int axis, unsigned short edge, bool updateOverlaps = true); + void sortMaxUp(int axis, unsigned short edge, bool updateOverlaps = true); + +public: + btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384); + virtual ~btAxisSweep3(); + + virtual void refreshOverlappingPairs() + { + //this is replace by sweep and prune + } + + unsigned short addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); + void removeHandle(unsigned short handle); + void updateHandle(unsigned short handle, const btPoint3& aabbMin,const btPoint3& aabbMax); + inline Handle* getHandle(unsigned short index) const {return m_pHandles + index;} + + + //Broadphase Interface + virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); + virtual void destroyProxy(btBroadphaseProxy* proxy); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); + +}; + +#endif //AXIS_SWEEP_3_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h new file mode 100644 index 00000000000..0c0bfe4f7b9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BROADPHASE_INTERFACE_H +#define BROADPHASE_INTERFACE_H + + + +struct btDispatcherInfo; +class btDispatcher; +struct btBroadphaseProxy; +#include "LinearMath/btVector3.h" + +///BroadphaseInterface for aabb-overlapping object pairs +class btBroadphaseInterface +{ +public: + virtual ~btBroadphaseInterface() {} + + virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0; + virtual void destroyProxy(btBroadphaseProxy* proxy)=0; + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0; + virtual void cleanProxyFromPairs(btBroadphaseProxy* proxy)=0; + + +}; + +#endif //BROADPHASE_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp new file mode 100644 index 00000000000..f4d7341f8dd --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp @@ -0,0 +1,17 @@ +/* +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 "btBroadphaseProxy.h" + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h new file mode 100644 index 00000000000..713d7d1aa19 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -0,0 +1,177 @@ +/* +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 BROADPHASE_PROXY_H +#define BROADPHASE_PROXY_H + + + +/// btDispatcher uses these types +/// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave +/// to facilitate type checking +enum BroadphaseNativeTypes +{ +// polyhedral convex shapes + BOX_SHAPE_PROXYTYPE, + TRIANGLE_SHAPE_PROXYTYPE, + TETRAHEDRAL_SHAPE_PROXYTYPE, + CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, + CONVEX_HULL_SHAPE_PROXYTYPE, +//implicit convex shapes +IMPLICIT_CONVEX_SHAPES_START_HERE, + SPHERE_SHAPE_PROXYTYPE, + MULTI_SPHERE_SHAPE_PROXYTYPE, + CONE_SHAPE_PROXYTYPE, + CONVEX_SHAPE_PROXYTYPE, + CYLINDER_SHAPE_PROXYTYPE, + MINKOWSKI_SUM_SHAPE_PROXYTYPE, + MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, +//concave shapes +CONCAVE_SHAPES_START_HERE, + //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! + TRIANGLE_MESH_SHAPE_PROXYTYPE, + ///used for demo integration FAST/Swift collision library and Bullet + FAST_CONCAVE_MESH_PROXYTYPE, + + EMPTY_SHAPE_PROXYTYPE, + STATIC_PLANE_PROXYTYPE, +CONCAVE_SHAPES_END_HERE, + + COMPOUND_SHAPE_PROXYTYPE, + + MAX_BROADPHASE_COLLISION_TYPES +}; + + +///btBroadphaseProxy +struct btBroadphaseProxy +{ + + ///optional filtering to cull potential collisions + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter, + }; + + //Usually the client btCollisionObject or Rigidbody class + void* m_clientObject; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + //used for memory pools + btBroadphaseProxy() :m_clientObject(0){} + + btBroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) + :m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask) + { + } + + static inline bool isPolyhedral(int proxyType) + { + return (proxyType < IMPLICIT_CONVEX_SHAPES_START_HERE); + } + + static inline bool isConvex(int proxyType) + { + return (proxyType < CONCAVE_SHAPES_START_HERE); + } + + static inline bool isConcave(int proxyType) + { + return ((proxyType > CONCAVE_SHAPES_START_HERE) && + (proxyType < CONCAVE_SHAPES_END_HERE)); + } + static inline bool isCompound(int proxyType) + { + return (proxyType == COMPOUND_SHAPE_PROXYTYPE); + } + +}; + +class btCollisionAlgorithm; + +struct btBroadphaseProxy; + +//Increase SIMPLE_MAX_ALGORITHMS to allow multiple btDispatchers caching their own algorithms +#define SIMPLE_MAX_ALGORITHMS 1 + +/// contains a pair of aabb-overlapping objects +struct btBroadphasePair +{ + btBroadphasePair () + : + m_pProxy0(0), + m_pProxy1(0) + { + for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++) + { + m_algorithms[i] = 0; + } + } + + btBroadphasePair(const btBroadphasePair& other) + : m_pProxy0(other.m_pProxy0), + m_pProxy1(other.m_pProxy1) + { + for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++) + { + m_algorithms[i] = other.m_algorithms[i]; + } + } + btBroadphasePair(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1) + { + + //keep them sorted, so the std::set operations work + if (&proxy0 < &proxy1) + { + m_pProxy0 = &proxy0; + m_pProxy1 = &proxy1; + } + else + { + m_pProxy0 = &proxy1; + m_pProxy1 = &proxy0; + } + + for (int i=0;i<SIMPLE_MAX_ALGORITHMS;i++) + { + m_algorithms[i] = 0; + } + + } + + btBroadphaseProxy* m_pProxy0; + btBroadphaseProxy* m_pProxy1; + + mutable btCollisionAlgorithm* m_algorithms[SIMPLE_MAX_ALGORITHMS]; +}; + + +//comparison for set operation, see Solid DT_Encounter +inline bool operator<(const btBroadphasePair& a, const btBroadphasePair& b) +{ + return a.m_pProxy0 < b.m_pProxy0 || + (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 < b.m_pProxy1); +} + + +#endif //BROADPHASE_PROXY_H + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp new file mode 100644 index 00000000000..2ad0c86d8a2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -0,0 +1,23 @@ +/* +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 "btCollisionAlgorithm.h" +#include "btDispatcher.h" + +btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) +{ + m_dispatcher = ci.m_dispatcher; +} + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h new file mode 100644 index 00000000000..a57c7619985 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -0,0 +1,70 @@ +/* +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 COLLISION_ALGORITHM_H +#define COLLISION_ALGORITHM_H + +struct btBroadphaseProxy; +class btDispatcher; +class btManifoldResult; +struct btCollisionObject; +struct btDispatcherInfo; + +struct btCollisionAlgorithmConstructionInfo +{ + btCollisionAlgorithmConstructionInfo() + :m_dispatcher(0) + { + } + btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) + :m_dispatcher(dispatcher) + { + } + + btDispatcher* m_dispatcher; + + int getDispatcherId(); + +}; + + +///btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatcher. +///It is persistent over frames +class btCollisionAlgorithm +{ + +protected: + + btDispatcher* m_dispatcher; + +protected: + int getDispatcherId(); + +public: + + btCollisionAlgorithm() {}; + + btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); + + virtual ~btCollisionAlgorithm() {}; + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + + virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) = 0; + +}; + + +#endif //COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp new file mode 100644 index 00000000000..20768225b3a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.cpp @@ -0,0 +1,22 @@ +/* +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 "btDispatcher.h" + +btDispatcher::~btDispatcher() +{ + +} + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h new file mode 100644 index 00000000000..f87c0281a97 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -0,0 +1,94 @@ +/* +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 _DISPATCHER_H +#define _DISPATCHER_H + +class btCollisionAlgorithm; +struct btBroadphaseProxy; +class btRigidBody; +struct btCollisionObject; +class btOverlappingPairCache; + +enum btCollisionDispatcherId +{ + RIGIDBODY_DISPATCHER = 0, + USERCALLBACK_DISPATCHER +}; + +class btPersistentManifold; + +struct btDispatcherInfo +{ + enum DispatchFunc + { + DISPATCH_DISCRETE = 1, + DISPATCH_CONTINUOUS + }; + btDispatcherInfo() + :m_dispatchFunc(DISPATCH_DISCRETE), + m_timeOfImpact(1.f), + m_useContinuous(false), + m_debugDraw(0), + m_enableSatConvex(false) + { + + } + float m_timeStep; + int m_stepCount; + int m_dispatchFunc; + float m_timeOfImpact; + bool m_useContinuous; + class btIDebugDraw* m_debugDraw; + bool m_enableSatConvex; + +}; + +/// btDispatcher can be used in combination with broadphase to dispatch overlapping pairs. +/// For example for pairwise collision detection or user callbacks (game logic). +class btDispatcher +{ + + +public: + virtual ~btDispatcher() ; + + virtual btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) = 0; + + // + // asume dispatchers to have unique id's in the range [0..max dispacher] + // + virtual int getUniqueId() = 0; + + virtual btPersistentManifold* getNewManifold(void* body0,void* body1)=0; + + virtual void releaseManifold(btPersistentManifold* manifold)=0; + + virtual void clearManifold(btPersistentManifold* manifold)=0; + + virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1) = 0; + + virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0; + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0; + + virtual int getNumManifolds() const = 0; + + virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0; + +}; + + +#endif //_DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp new file mode 100644 index 00000000000..8950d20f22e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -0,0 +1,203 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "btOverlappingPairCache.h" + +#include "btDispatcher.h" +#include "btCollisionAlgorithm.h" + +int gOverlappingPairs = 0; + +btOverlappingPairCache::btOverlappingPairCache(): +m_blockedForChanges(false) +//m_NumOverlapBroadphasePair(0) +{ +} + + +btOverlappingPairCache::~btOverlappingPairCache() +{ + //todo/test: show we erase/delete data, or is it automatic +} + + +void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) +{ + + std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(findPair); +// assert(it != m_overlappingPairSet.end()); + + if (it != m_overlappingPairSet.end()) + { + gOverlappingPairs--; + btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); + cleanOverlappingPair(*pair); + m_overlappingPairSet.erase(it); + } +} + + +void btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair) +{ + for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++) + { + if (pair.m_algorithms[dispatcherId]) + { + { + delete pair.m_algorithms[dispatcherId]; + pair.m_algorithms[dispatcherId]=0; + } + } + } +} + + + + + +void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + //don't add overlap with own + assert(proxy0 != proxy1); + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return; + + + btBroadphasePair pair(*proxy0,*proxy1); + + m_overlappingPairSet.insert(pair); + gOverlappingPairs++; + +} + +///this findPair becomes really slow. Either sort the list to speedup the query, or +///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. +///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) +///Also we can use a 2D bitmap, which can be useful for a future GPU implementation + btBroadphasePair* btOverlappingPairCache::findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) +{ + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + btBroadphasePair tmpPair(*proxy0,*proxy1); + std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.find(tmpPair); + if ((it == m_overlappingPairSet.end())) + return 0; + + //assert(it != m_overlappingPairSet.end()); + btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); + return pair; +} + + + + + +void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy) +{ + + class CleanPairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_cleanProxy; + btOverlappingPairCache* m_pairCache; + + public: + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.m_pProxy0 == m_cleanProxy) || + (pair.m_pProxy1 == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this); + + processAllOverlappingPairs(&cleanPairs); + +} + + + +void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy) +{ + + class RemovePairCallback : public btOverlapCallback + { + btBroadphaseProxy* m_obsoleteProxy; + + public: + RemovePairCallback(btBroadphaseProxy* obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.m_pProxy0 == m_obsoleteProxy) || + (pair.m_pProxy1 == m_obsoleteProxy)); + } + + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback); +} + + + +void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback) +{ + std::set<btBroadphasePair>::iterator it = m_overlappingPairSet.begin(); + for (; !(it==m_overlappingPairSet.end());) + { + + btBroadphasePair* pair = (btBroadphasePair*)(&(*it)); + if (callback->processOverlap(*pair)) + { + cleanOverlappingPair(*pair); + + std::set<btBroadphasePair>::iterator it2 = it; + //why does next line not compile under OS X?? +#ifdef MAC_OSX_FIXED_STL_SET + it2++; + it = m_overlappingPairSet.erase(it); + assert(it == it2); +#else + it++; + m_overlappingPairSet.erase(it2); +#endif //MAC_OSX_FIXED_STL_SET + + gOverlappingPairs--; + } else + { + it++; + } + } +} + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h new file mode 100644 index 00000000000..85bb8265cf9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -0,0 +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 OVERLAPPING_PAIR_CACHE_H +#define OVERLAPPING_PAIR_CACHE_H + + +#include "btBroadphaseInterface.h" +#include "btBroadphaseProxy.h" +#include "LinearMath/btPoint3.h" +#include <set> + + +struct btOverlapCallback +{ +virtual ~btOverlapCallback() +{ +} + //return true for deletion of the pair + virtual bool processOverlap(btBroadphasePair& pair) = 0; +}; + +///btOverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase +class btOverlappingPairCache : public btBroadphaseInterface +{ + //avoid brute-force finding all the time + std::set<btBroadphasePair> m_overlappingPairSet; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + public: + + btOverlappingPairCache(); + virtual ~btOverlappingPairCache(); + + void processAllOverlappingPairs(btOverlapCallback*); + + void removeOverlappingPair(btBroadphasePair& pair); + + void cleanOverlappingPair(btBroadphasePair& pair); + + void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); + + + void cleanProxyFromPairs(btBroadphaseProxy* proxy); + + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); + + + inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const + { + bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask; + collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + + + virtual void refreshOverlappingPairs() =0; + + + + +}; +#endif //OVERLAPPING_PAIR_CACHE_H + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp new file mode 100644 index 00000000000..1a24c7a7a8c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -0,0 +1,220 @@ +/* +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 "btSimpleBroadphase.h" +#include <BulletCollision/BroadphaseCollision/btDispatcher.h> +#include <BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h> + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" +#include <vector> + + +void btSimpleBroadphase::validate() +{ + for (int i=0;i<m_numProxies;i++) + { + for (int j=i+1;j<m_numProxies;j++) + { + assert(m_pProxies[i] != m_pProxies[j]); + } + } + +} + +btSimpleBroadphase::btSimpleBroadphase(int maxProxies) + :btOverlappingPairCache(), + m_firstFreeProxy(0), + m_numProxies(0), + m_maxProxies(maxProxies) +{ + + m_proxies = new btSimpleBroadphaseProxy[maxProxies]; + m_freeProxies = new int[maxProxies]; + m_pProxies = new btSimpleBroadphaseProxy*[maxProxies]; + + + int i; + for (i=0;i<m_maxProxies;i++) + { + m_freeProxies[i] = i; + } +} + +btSimpleBroadphase::~btSimpleBroadphase() +{ + delete[] m_proxies; + delete []m_freeProxies; + delete [] m_pProxies; + + /*int i; + for (i=m_numProxies-1;i>=0;i--) + { + BP_Proxy* proxy = m_pProxies[i]; + destroyProxy(proxy); + } + */ +} + + +btBroadphaseProxy* btSimpleBroadphase::createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask) +{ + if (m_numProxies >= m_maxProxies) + { + assert(0); + return 0; //should never happen, but don't let the game crash ;-) + } + assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]); + + int freeIndex= m_freeProxies[m_firstFreeProxy]; + btSimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])btSimpleBroadphaseProxy(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask); + m_firstFreeProxy++; + + btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; + + int index = proxy - proxy1; + assert(index == freeIndex); + + m_pProxies[m_numProxies] = proxy; + m_numProxies++; + //validate(); + + return proxy; +} + +class RemovingOverlapCallback : public btOverlapCallback +{ +protected: + virtual bool processOverlap(btBroadphasePair& pair) + { + assert(0); + } +}; + +class RemovePairContainingProxy +{ + + btBroadphaseProxy* m_targetProxy; + public: + virtual ~RemovePairContainingProxy() + { + } +protected: + virtual bool processOverlap(btBroadphasePair& pair) + { + btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0); + btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1); + + return ((m_targetProxy == proxy0 || m_targetProxy == proxy1)); + }; +}; + +void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) +{ + + int i; + + btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg); + btSimpleBroadphaseProxy* proxy1 = &m_proxies[0]; + + int index = proxy0 - proxy1; + assert (index < m_maxProxies); + m_freeProxies[--m_firstFreeProxy] = index; + + removeOverlappingPairsContainingProxy(proxyOrg); + + for (i=0;i<m_numProxies;i++) + { + if (m_pProxies[i] == proxyOrg) + { + m_pProxies[i] = m_pProxies[m_numProxies-1]; + break; + } + } + m_numProxies--; + //validate(); + +} + +void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax) +{ + btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy); + sbp->m_min = aabbMin; + sbp->m_max = aabbMax; +} + + + + + + + + + +bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1) +{ + return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && + proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] && + proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2]; + +} + + + +//then remove non-overlapping ones +class CheckOverlapCallback : public btOverlapCallback +{ +public: + virtual bool processOverlap(btBroadphasePair& pair) + { + return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0),static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1))); + } +}; + +void btSimpleBroadphase::refreshOverlappingPairs() +{ + //first check for new overlapping pairs + int i,j; + + for (i=0;i<m_numProxies;i++) + { + btBroadphaseProxy* proxy0 = m_pProxies[i]; + for (j=i+1;j<m_numProxies;j++) + { + btBroadphaseProxy* proxy1 = m_pProxies[j]; + btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0); + btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1); + + if (aabbOverlap(p0,p1)) + { + if ( !findPair(proxy0,proxy1)) + { + addOverlappingPair(proxy0,proxy1); + } + } + + } + } + + + CheckOverlapCallback checkOverlap; + + processAllOverlappingPairs(&checkOverlap); + + +} + + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h new file mode 100644 index 00000000000..281677081d7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -0,0 +1,92 @@ +/* +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 SIMPLE_BROADPHASE_H +#define SIMPLE_BROADPHASE_H + + +#include "btOverlappingPairCache.h" + + +struct btSimpleBroadphaseProxy : public btBroadphaseProxy +{ + btVector3 m_min; + btVector3 m_max; + + btSimpleBroadphaseProxy() {}; + + btSimpleBroadphaseProxy(const btPoint3& minpt,const btPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask) + :btBroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask), + m_min(minpt),m_max(maxpt) + { + } + + +}; + +///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks +class btSimpleBroadphase : public btOverlappingPairCache +{ + + btSimpleBroadphaseProxy* m_proxies; + int* m_freeProxies; + int m_firstFreeProxy; + + btSimpleBroadphaseProxy** m_pProxies; + int m_numProxies; + + + + int m_maxProxies; + + + inline btSimpleBroadphaseProxy* getSimpleProxyFromProxy(btBroadphaseProxy* proxy) + { + btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy); + return proxy0; + } + + + void validate(); + +protected: + + + virtual void refreshOverlappingPairs(); +public: + btSimpleBroadphase(int maxProxies=16384); + virtual ~btSimpleBroadphase(); + + + static bool aabbOverlap(btSimpleBroadphaseProxy* proxy0,btSimpleBroadphaseProxy* proxy1); + + + virtual btBroadphaseProxy* createProxy( const btVector3& min, const btVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); + + + virtual void destroyProxy(btBroadphaseProxy* proxy); + virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); + + + + + + +}; + + + +#endif //SIMPLE_BROADPHASE_H + diff --git a/extern/bullet2/src/BulletCollision/CMakeLists.txt b/extern/bullet2/src/BulletCollision/CMakeLists.txt new file mode 100644 index 00000000000..d10bd249149 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CMakeLists.txt @@ -0,0 +1,57 @@ + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src } +) + +ADD_LIBRARY(LibBulletCollision + BroadphaseCollision/btAxisSweep3.cpp + BroadphaseCollision/btBroadphaseProxy.cpp + BroadphaseCollision/btCollisionAlgorithm.cpp + BroadphaseCollision/btDispatcher.cpp + BroadphaseCollision/btOverlappingPairCache.cpp + BroadphaseCollision/btSimpleBroadphase.cpp + CollisionDispatch/btCollisionDispatcher.cpp + CollisionDispatch/btCollisionObject.cpp + CollisionDispatch/btCollisionWorld.cpp + CollisionDispatch/btCompoundCollisionAlgorithm.cpp + CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp + CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp + CollisionDispatch/btConvexConvexAlgorithm.cpp + CollisionDispatch/btEmptyCollisionAlgorithm.cpp + CollisionDispatch/btManifoldResult.cpp + CollisionDispatch/btSimulationIslandManager.cpp + CollisionDispatch/btUnionFind.cpp + CollisionShapes/btBoxShape.cpp + CollisionShapes/btBvhTriangleMeshShape.cpp + CollisionShapes/btCollisionShape.cpp + CollisionShapes/btCompoundShape.cpp + CollisionShapes/btConcaveShape.cpp + CollisionShapes/btConeShape.cpp + CollisionShapes/btConvexHullShape.cpp + CollisionShapes/btConvexShape.cpp + CollisionShapes/btConvexTriangleMeshShape.cpp + CollisionShapes/btCylinderShape.cpp + CollisionShapes/btEmptyShape.cpp + CollisionShapes/btMinkowskiSumShape.cpp + CollisionShapes/btMultiSphereShape.cpp + CollisionShapes/btOptimizedBvh.cpp + CollisionShapes/btPolyhedralConvexShape.cpp + CollisionShapes/btTetrahedronShape.cpp + CollisionShapes/btSphereShape.cpp + CollisionShapes/btStaticPlaneShape.cpp + CollisionShapes/btStridingMeshInterface.cpp + CollisionShapes/btTriangleCallback.cpp + CollisionShapes/btTriangleIndexVertexArray.cpp + CollisionShapes/btTriangleMesh.cpp + CollisionShapes/btTriangleMeshShape.cpp + NarrowPhaseCollision/btContinuousConvexCollision.cpp + NarrowPhaseCollision/btConvexCast.cpp + NarrowPhaseCollision/btGjkConvexCast.cpp + NarrowPhaseCollision/btGjkPairDetector.cpp + NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp + NarrowPhaseCollision/btPersistentManifold.cpp + NarrowPhaseCollision/btRaycastCallback.cpp + NarrowPhaseCollision/btSubSimplexConvexCast.cpp + NarrowPhaseCollision/btVoronoiSimplexSolver.cpp +) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h new file mode 100644 index 00000000000..6d499f508e9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -0,0 +1,44 @@ +/* +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 COLLISION_CREATE_FUNC +#define COLLISION_CREATE_FUNC + +#include <vector> + +typedef std::vector<struct btCollisionObject*> btCollisionObjectArray; +class btCollisionAlgorithm; +struct btCollisionObject; + +struct btCollisionAlgorithmConstructionInfo; + +///Used by the btCollisionDispatcher to register and create instances for btCollisionAlgorithm +struct btCollisionAlgorithmCreateFunc +{ + bool m_swapped; + + btCollisionAlgorithmCreateFunc() + :m_swapped(false) + { + } + virtual ~btCollisionAlgorithmCreateFunc(){}; + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return 0; + } +}; +#endif //COLLISION_CREATE_FUNC + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp new file mode 100644 index 00000000000..d824f68ebe7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -0,0 +1,336 @@ +/* +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 "btCollisionDispatcher.h" + + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include <algorithm> +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" + +int gNumManifold = 0; + +#include <stdio.h> + + +btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms) +:m_useIslands(true), +m_count(0), +m_convexConvexCreateFunc(0), +m_convexConcaveCreateFunc(0), +m_swappedConvexConcaveCreateFunc(0), +m_compoundCreateFunc(0), +m_swappedCompoundCreateFunc(0), +m_emptyCreateFunc(0) +{ + int i; + + m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; + for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + { + for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + { + m_doubleDispatch[i][j] = m_emptyCreateFunc; + } + } +} + + +btCollisionDispatcher::btCollisionDispatcher (): + m_useIslands(true), + m_count(0) +{ + int i; + + //default CreationFunctions, filling the m_doubleDispatch table + m_convexConvexCreateFunc = new btConvexConvexAlgorithm::CreateFunc; + m_convexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::CreateFunc; + m_swappedConvexConcaveCreateFunc = new btConvexConcaveCollisionAlgorithm::SwappedCreateFunc; + m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc; + m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc; + m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; + + for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++) + { + for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++) + { + m_doubleDispatch[i][j] = internalFindCreateFunc(i,j); + assert(m_doubleDispatch[i][j]); + } + } + + +}; + +void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +{ + m_doubleDispatch[proxyType0][proxyType1] = createFunc; +} + +btCollisionDispatcher::~btCollisionDispatcher() +{ + delete m_convexConvexCreateFunc; + delete m_convexConcaveCreateFunc; + delete m_swappedConvexConcaveCreateFunc; + delete m_compoundCreateFunc; + delete m_swappedCompoundCreateFunc; + delete m_emptyCreateFunc; +} + +btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) +{ + gNumManifold++; + + //ASSERT(gNumManifold < 65535); + + + btCollisionObject* body0 = (btCollisionObject*)b0; + btCollisionObject* body1 = (btCollisionObject*)b1; + + btPersistentManifold* manifold = new btPersistentManifold (body0,body1); + m_manifoldsPtr.push_back(manifold); + + return manifold; +} + +void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) +{ + manifold->clearManifold(); +} + + +void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) +{ + + gNumManifold--; + + //printf("releaseManifold: gNumManifold %d\n",gNumManifold); + + clearManifold(manifold); + + std::vector<btPersistentManifold*>::iterator i = + std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold); + if (!(i == m_manifoldsPtr.end())) + { + std::swap(*i, m_manifoldsPtr.back()); + m_manifoldsPtr.pop_back(); + delete manifold; + + } + + +} + + + +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1) +{ +#define USE_DISPATCH_REGISTRY_ARRAY 1 +#ifdef USE_DISPATCH_REGISTRY_ARRAY + + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = this; + btCollisionAlgorithm* algo = m_doubleDispatch[body0->m_collisionShape->getShapeType()][body1->m_collisionShape->getShapeType()] + ->CreateCollisionAlgorithm(ci,body0,body1); +#else + btCollisionAlgorithm* algo = internalFindAlgorithm(body0,body1); +#endif //USE_DISPATCH_REGISTRY_ARRAY + return algo; +} + + +btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1) +{ + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; +} + + + +btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1) +{ + m_count++; + + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = this; + + if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConvex() ) + { + return new btConvexConvexAlgorithm(0,ci,body0,body1); + } + + if (body0->m_collisionShape->isConvex() && body1->m_collisionShape->isConcave()) + { + return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); + } + + if (body1->m_collisionShape->isConvex() && body0->m_collisionShape->isConcave()) + { + return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); + } + + if (body0->m_collisionShape->isCompound()) + { + return new btCompoundCollisionAlgorithm(ci,body0,body1,false); + } else + { + if (body1->m_collisionShape->isCompound()) + { + return new btCompoundCollisionAlgorithm(ci,body0,body1,true); + } + } + + //failed to find an algorithm + return new btEmptyAlgorithm(ci); + +} + +bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1) +{ + //here you can do filtering + bool hasResponse = + (body0->hasContactResponse() && body1->hasContactResponse()); + //no response between two static/kinematic bodies: + hasResponse = hasResponse && + ((!body0->isStaticOrKinematicObject()) ||(! body1->isStaticOrKinematicObject())); + return hasResponse; +} + +bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1) +{ + assert(body0); + assert(body1); + + bool needsCollision = true; + + //broadphase filtering already deals with this + if ((body0->isStaticObject() || body0->isKinematicObject()) && + (body1->isStaticObject() || body1->isKinematicObject())) + { + printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); + } + + if ((!body0->IsActive()) && (!body1->IsActive())) + needsCollision = false; + + return needsCollision ; + +} + + + +///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc) +///this is useful for the collision dispatcher. +class btCollisionPairCallback : public btOverlapCallback +{ + btDispatcherInfo& m_dispatchInfo; + btCollisionDispatcher* m_dispatcher; + int m_dispatcherId; +public: + + btCollisionPairCallback(btDispatcherInfo& dispatchInfo,btCollisionDispatcher* dispatcher,int dispatcherId) + :m_dispatchInfo(dispatchInfo), + m_dispatcher(dispatcher), + m_dispatcherId(dispatcherId) + { + } + + virtual bool processOverlap(btBroadphasePair& pair) + { + btCollisionObject* body0 = (btCollisionObject*)pair.m_pProxy0->m_clientObject; + btCollisionObject* body1 = (btCollisionObject*)pair.m_pProxy1->m_clientObject; + + if (!m_dispatcher->needsCollision(body0,body1)) + return false; + + //dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithms[m_dispatcherId]) + { + pair.m_algorithms[m_dispatcherId] = m_dispatcher->findAlgorithm( + body0, + body1); + } + + if (pair.m_algorithms[m_dispatcherId]) + { + btManifoldResult* resultOut = m_dispatcher->internalGetNewManifoldResult(body0,body1); + if (m_dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE) + { + + pair.m_algorithms[m_dispatcherId]->processCollision(body0,body1,m_dispatchInfo,resultOut); + } else + { + float toi = pair.m_algorithms[m_dispatcherId]->calculateTimeOfImpact(body0,body1,m_dispatchInfo,resultOut); + if (m_dispatchInfo.m_timeOfImpact > toi) + m_dispatchInfo.m_timeOfImpact = toi; + + } + m_dispatcher->internalReleaseManifoldResult(resultOut); + } + return false; + + } +}; + + +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) +{ + //m_blockedForChanges = true; + + int dispatcherId = getUniqueId(); + + btCollisionPairCallback collisionCallback(dispatchInfo,this,dispatcherId); + + pairCache->processAllOverlappingPairs(&collisionCallback); + + //m_blockedForChanges = false; + +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h new file mode 100644 index 00000000000..def59650455 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -0,0 +1,132 @@ +/* +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 COLLISION__DISPATCHER_H +#define COLLISION__DISPATCHER_H + +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" + + +class btIDebugDraw; +class btOverlappingPairCache; + + +#include "btCollisionCreateFunc.h" + + + + +///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. +///Time of Impact, Closest Points and Penetration Depth. +class btCollisionDispatcher : public btDispatcher +{ + + std::vector<btPersistentManifold*> m_manifoldsPtr; + + bool m_useIslands; + + btManifoldResult m_defaultManifoldResult; + + btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + + btCollisionAlgorithmCreateFunc* internalFindCreateFunc(int proxyType0,int proxyType1); + + //default CreationFunctions, filling the m_doubleDispatch table + btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; + btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; + btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; + btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + + btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1); + +public: + + ///allows the user to get contact point callbacks + inline btManifoldResult* internalGetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1) + { + //in-place, this prevents parallel dispatching, but just adding a list would fix that. + btManifoldResult* manifoldResult = new (&m_defaultManifoldResult) btManifoldResult(obj0,obj1); + return manifoldResult; + } + + ///allows the user to get contact point callbacks + inline void internalReleaseManifoldResult(btManifoldResult*) + { + } + + ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions + void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + + int getNumManifolds() const + { + return m_manifoldsPtr.size(); + } + + btPersistentManifold** getInternalManifoldPointer() + { + return &m_manifoldsPtr[0]; + } + + btPersistentManifold* getManifoldByIndexInternal(int index) + { + return m_manifoldsPtr[index]; + } + + const btPersistentManifold* getManifoldByIndexInternal(int index) const + { + return m_manifoldsPtr[index]; + } + + int m_count; + + ///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support + btCollisionDispatcher (); + + ///a special constructor that doesn't create/register the default collision algorithms + btCollisionDispatcher(bool noDefaultAlgorithms); + + virtual ~btCollisionDispatcher(); + + virtual btPersistentManifold* getNewManifold(void* b0,void* b1); + + virtual void releaseManifold(btPersistentManifold* manifold); + + + virtual void clearManifold(btPersistentManifold* manifold); + + + btCollisionAlgorithm* findAlgorithm(btCollisionObject* body0,btCollisionObject* body1); + + virtual bool needsCollision(btCollisionObject* body0,btCollisionObject* body1); + + virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1); + + virtual int getUniqueId() { return RIGIDBODY_DISPATCHER;} + + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo); + + + +}; + +#endif //COLLISION__DISPATCHER_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp new file mode 100644 index 00000000000..881a8c0042a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -0,0 +1,53 @@ +/* +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 "btCollisionObject.h" + +btCollisionObject::btCollisionObject() + : m_broadphaseHandle(0), + m_collisionShape(0), + m_collisionFlags(0), + m_activationState1(1), + m_deactivationTime(0.f), + m_userObjectPointer(0), + m_hitFraction(1.f), + m_ccdSweptSphereRadius(0.f), + m_ccdSquareMotionTreshold(0.f) +{ + +} + + +void btCollisionObject::SetActivationState(int newState) +{ + if ( (m_activationState1 != DISABLE_DEACTIVATION) && (m_activationState1 != DISABLE_SIMULATION)) + m_activationState1 = newState; +} + +void btCollisionObject::ForceActivationState(int newState) +{ + m_activationState1 = newState; +} + +void btCollisionObject::activate() +{ + if (!(m_collisionFlags & (CF_STATIC_OBJECT|CF_KINEMATIC_OJBECT))) + { + SetActivationState(ACTIVE_TAG); + m_deactivationTime = 0.f; + } +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h new file mode 100644 index 00000000000..3838fc98961 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -0,0 +1,148 @@ +/* +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 COLLISION_OBJECT_H +#define COLLISION_OBJECT_H + +#include "LinearMath/btTransform.h" + +//island management, m_activationState1 +#define ACTIVE_TAG 1 +#define ISLAND_SLEEPING 2 +#define WANTS_DEACTIVATION 3 +#define DISABLE_DEACTIVATION 4 +#define DISABLE_SIMULATION 5 + +struct btBroadphaseProxy; +class btCollisionShape; +#include "LinearMath/btMotionState.h" + + + +/// btCollisionObject can be used to manage collision detection objects. +/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. +/// They can be added to the btCollisionWorld. +struct btCollisionObject +{ + btTransform m_worldTransform; + btBroadphaseProxy* m_broadphaseHandle; + btCollisionShape* m_collisionShape; + + ///m_interpolationWorldTransform is used for CCD and interpolation + ///it can be either previous or future (predicted) transform + btTransform m_interpolationWorldTransform; + + enum CollisionFlags + { + CF_STATIC_OBJECT= 1, + CF_KINEMATIC_OJBECT= 2, + CF_NO_CONTACT_RESPONSE = 4, + CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution) + }; + + int m_collisionFlags; + + int m_islandTag1; + int m_activationState1; + float m_deactivationTime; + + btScalar m_friction; + btScalar m_restitution; + + ///users can point to their objects, m_userPointer is not used by Bullet + void* m_userObjectPointer; + + ///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead. + void* m_internalOwner; + + ///time of impact calculation + float m_hitFraction; + + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + float m_ccdSweptSphereRadius; + + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold + float m_ccdSquareMotionTreshold; + + inline bool mergesSimulationIslands() const + { + ///static objects, kinematic and object without contact response don't merge islands + return !(m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OJBECT | CF_NO_CONTACT_RESPONSE) ); + } + + + inline bool isStaticObject() const { + return m_collisionFlags & CF_STATIC_OBJECT; + } + + inline bool isKinematicObject() const + { + return m_collisionFlags & CF_KINEMATIC_OJBECT; + } + + inline bool isStaticOrKinematicObject() const + { + return m_collisionFlags & (CF_KINEMATIC_OJBECT | CF_STATIC_OBJECT); + } + + inline bool hasContactResponse() const { + return !(m_collisionFlags & CF_NO_CONTACT_RESPONSE); + } + + + + + btCollisionObject(); + + + void SetCollisionShape(btCollisionShape* collisionShape) + { + m_collisionShape = collisionShape; + } + + int GetActivationState() const { return m_activationState1;} + + void SetActivationState(int newState); + + void ForceActivationState(int newState); + + void activate(); + + inline bool IsActive() const + { + return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION)); + } + + void setRestitution(float rest) + { + m_restitution = rest; + } + float getRestitution() const + { + return m_restitution; + } + void setFriction(float frict) + { + m_friction = frict; + } + float getFriction() const + { + return m_friction; + } + + +}; + +#endif //COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp new file mode 100644 index 00000000000..e24a5c6a0b4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -0,0 +1,356 @@ +/* +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 "btCollisionWorld.h" +#include "btCollisionDispatcher.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btQuickprof.h" + +//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" + +#include <algorithm> + +btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) +:m_dispatcher1(dispatcher), +m_broadphasePairCache(pairCache), +m_ownsDispatcher(false), +m_ownsBroadphasePairCache(false) +{ +} + +btCollisionWorld::btCollisionWorld() +: m_dispatcher1(new btCollisionDispatcher()), +m_broadphasePairCache(new btSimpleBroadphase()), +m_ownsDispatcher(true), +m_ownsBroadphasePairCache(true) +{ +} + + +btCollisionWorld::~btCollisionWorld() +{ + //clean up remaining objects + std::vector<btCollisionObject*>::iterator i; + + for (i=m_collisionObjects.begin(); + !(i==m_collisionObjects.end()); i++) + + { + btCollisionObject* collisionObject= (*i); + + btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + getBroadphase()->cleanProxyFromPairs(bp); + getBroadphase()->destroyProxy(bp); + } + } + + if (m_ownsDispatcher) + delete m_dispatcher1; + if (m_ownsBroadphasePairCache) + delete m_broadphasePairCache; + +} + + + + + + + + + + +void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +{ + + //check that the object isn't already added + std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); + assert(i == m_collisionObjects.end()); + + + m_collisionObjects.push_back(collisionObject); + + //calculate new AABB + btTransform trans = collisionObject->m_worldTransform; + + btVector3 minAabb; + btVector3 maxAabb; + collisionObject->m_collisionShape->getAabb(trans,minAabb,maxAabb); + + int type = collisionObject->m_collisionShape->getShapeType(); + collisionObject->m_broadphaseHandle = getBroadphase()->createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask + ); + + + + +} + +void btCollisionWorld::performDiscreteCollisionDetection() +{ + BEGIN_PROFILE("performDiscreteCollisionDetection"); + + btDispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = 0.f; + dispatchInfo.m_stepCount = 0; + + //update aabb (of all moved objects) + + btVector3 aabbMin,aabbMax; + for (size_t i=0;i<m_collisionObjects.size();i++) + { + m_collisionObjects[i]->m_collisionShape->getAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax); + m_broadphasePairCache->setAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); + } + + m_broadphasePairCache->refreshOverlappingPairs(); + + btDispatcher* dispatcher = getDispatcher(); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache,dispatchInfo); + + END_PROFILE("performDiscreteCollisionDetection"); + +} + + +void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + + + //bool removeFromBroadphase = false; + + { + + btBroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + getBroadphase()->cleanProxyFromPairs(bp); + getBroadphase()->destroyProxy(bp); + collisionObject->m_broadphaseHandle = 0; + } + } + + + std::vector<btCollisionObject*>::iterator i = std::find(m_collisionObjects.begin(), m_collisionObjects.end(), collisionObject); + + if (!(i == m_collisionObjects.end())) + { + std::swap(*i, m_collisionObjects.back()); + m_collisionObjects.pop_back(); + } +} + +void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) +{ + + btSphereShape pointShape(0.0f); + + if (collisionShape->isConvex()) + { + btConvexCast::CastResult castResult; + castResult.m_fraction = 1.f;//?? + + btConvexShape* convexShape = (btConvexShape*) collisionShape; + btVoronoiSimplexSolver simplexSolver; + btSubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > 0.0001f) + { + castResult.m_normal.normalize(); + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + + btCollisionWorld::LocalRayResult localRayResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction + ); + + resultCallback.AddSingleResult(localRayResult); + + } + } + } + } + else + { + + if (collisionShape->isConcave()) + { + + btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; + + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + + btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback + { + btCollisionWorld::RayResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btTriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, + btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): + btTriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + btCollisionWorld::LocalRayResult rayResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitFraction); + + return m_resultCallback->AddSingleResult(rayResult); + + + } + + }; + + + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + rcb.m_hitFraction = resultCallback.m_closestHitFraction; + + btVector3 rayAabbMinLocal = rayFromLocal; + rayAabbMinLocal.setMin(rayToLocal); + btVector3 rayAabbMaxLocal = rayFromLocal; + rayAabbMaxLocal.setMax(rayToLocal); + + triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + + } else + { + //todo: use AABB tree or other BVH acceleration structure! + if (collisionShape->isCompound()) + { + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); + int i=0; + for (i=0;i<compoundShape->getNumChildShapes();i++) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); + btTransform childWorldTrans = colObjWorldTransform * childTrans; + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback); + + } + + + } + } + } +} + +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) +{ + + + btTransform rayFromTrans,rayToTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + rayToTrans.setIdentity(); + + rayToTrans.setOrigin(rayToWorld); + + //do culling based on aabb (rayFrom/rayTo) + btVector3 rayAabbMin = rayFromWorld; + btVector3 rayAabbMax = rayFromWorld; + rayAabbMin.setMin(rayToWorld); + rayAabbMax.setMax(rayToWorld); + + + /// brute force go over all objects. Once there is a broadphase, use that, or + /// add a raycast against aabb first. + + std::vector<btCollisionObject*>::iterator iter; + + for (iter=m_collisionObjects.begin(); + !(iter==m_collisionObjects.end()); iter++) + { + + btCollisionObject* collisionObject= (*iter); + + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->m_collisionShape->getAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); + + //check aabb overlap + + if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + { + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->m_collisionShape, + collisionObject->m_worldTransform, + resultCallback); + + } + } + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h new file mode 100644 index 00000000000..2280399b2c8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -0,0 +1,244 @@ +/* +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. +*/ + + +/** + * @mainpage Bullet Documentation + * + * @section intro_sec Introduction + * Bullet Collision Detection & Physics SDK + * + * Bullet is a Collision Detection and Rigid Body Dynamics Library. The Library is Open Source and free for commercial use, under the ZLib license ( http://opensource.org/licenses/zlib-license.php ). + * + * There is the Physics Forum for Feedback and bteral Collision Detection and Physics discussions. + * Please visit http://www.continuousphysics.com/Bullet/phpBB2/index.php + * + * @section install_sec Installation + * + * @subsection step1 Step 1: Download + * You can download the Bullet Physics Library from our website: http://www.continuousphysics.com/Bullet/ + * @subsection step2 Step 2: Building + * Bullet comes with autogenerated Project Files for Microsoft Visual Studio 6, 7, 7.1 and 8. + * The main Workspace/Solution is located in Bullet/msvc/8/wksbullet.sln (replace 8 with your version). + * + * Under other platforms, like Linux or Mac OS-X, Bullet can be build using either using cmake, http://www.cmake.org, or jam, http://www.perforce.com/jam/jam.html . cmake can autogenerate Xcode, KDevelop, MSVC and other build systems. just run cmake . in the root of Bullet. + * Jam is a build system that can build the library, demos and also autogenerate the MSVC Project Files. + * So if you are not using MSVC, you can run configure and jam . + * If you don't have jam installed, you can make jam from the included jam-2.5 sources, or download jam from ftp://ftp.perforce.com/pub/jam/ + * + * @subsection step3 Step 3: Testing demos + * Try to run and experiment with CcdPhysicsDemo executable as a starting point. + * Bullet can be used in several ways, as Full Rigid Body simulation, as Collision Detector Library or Low Level / Snippets like the GJK Closest Point calculation. + * The Dependencies can be seen in this documentation under Directories + * + * @subsection step4 Step 4: Integrating in your application, Full Rigid Body Simulation + * Check out CcdPhysicsDemo how to create a btDynamicsWorld, btRigidBody and btCollisionShape, Stepping the simulation and synchronizing your graphics object transform. + * PLEASE NOTE THE CcdPhysicsEnvironment and CcdPhysicsController is obsolete and will be removed. It has been replaced by classes derived frmo btDynamicsWorld and btRididBody + * @subsection step5 Step 5 : Integrate the Collision Detection Library (without Dynamics and other Extras) + * Bullet Collision Detection can also be used without the Dynamics/Extras. + * Check out btCollisionWorld and btCollisionObject, and the CollisionInterfaceDemo. Also in Extras/test_BulletOde.cpp there is a sample Collision Detection integration with Open Dynamics Engine, ODE, http://www.ode.org + * @subsection step6 Step 6 : Use Snippets like the GJK Closest Point calculation. + * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. + * + * @section copyright Copyright + * Copyright (C) 2005-2006 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon + * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, + * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren. + * + */ + + + +#ifndef COLLISION_WORLD_H +#define COLLISION_WORLD_H + + +class btCollisionShape; +class btBroadphaseInterface; +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "btCollisionObject.h" +#include "btCollisionDispatcher.h" //for definition of btCollisionObjectArray +#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" + +#include <vector> + + + +///CollisionWorld is interface and container for the collision detection +class btCollisionWorld +{ + + +protected: + + std::vector<btCollisionObject*> m_collisionObjects; + + btDispatcher* m_dispatcher1; + + btOverlappingPairCache* m_broadphasePairCache; + + bool m_ownsDispatcher; + bool m_ownsBroadphasePairCache; + +public: + + //this constructor will create and own a dispatcher and paircache and delete it at destruction + btCollisionWorld(); + + //this constructor doesn't own the dispatcher and paircache/broadphase + btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache); + + virtual ~btCollisionWorld(); + + + btBroadphaseInterface* getBroadphase() + { + return m_broadphasePairCache; + } + + btOverlappingPairCache* getPairCache() + { + return m_broadphasePairCache; + } + + + btDispatcher* getDispatcher() + { + return m_dispatcher1; + } + + ///LocalShapeInfo gives extra information for complex shapes + ///Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart + struct LocalShapeInfo + { + int m_shapePart; + int m_triangleIndex; + + //const btCollisionShape* m_shapeTemp; + //const btTransform* m_shapeLocalTransform; + }; + + struct LocalRayResult + { + LocalRayResult(btCollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const btVector3& hitNormalLocal, + float hitFraction) + :m_collisionObject(collisionObject), + m_localShapeInfo(m_localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) + { + } + + btCollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + const btVector3& m_hitNormalLocal; + float m_hitFraction; + + }; + + ///RayResultCallback is used to report new raycast results + struct RayResultCallback + { + virtual ~RayResultCallback() + { + } + float m_closestHitFraction; + bool HasHit() + { + return (m_closestHitFraction < 1.f); + } + + RayResultCallback() + :m_closestHitFraction(1.f) + { + } + virtual float AddSingleResult(LocalRayResult& rayResult) = 0; + }; + + struct ClosestRayResultCallback : public RayResultCallback + { + ClosestRayResultCallback(btVector3 rayFromWorld,btVector3 rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_collisionObject(0) + { + } + + btVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + btVector3 m_rayToWorld; + + btVector3 m_hitNormalWorld; + btVector3 m_hitPointWorld; + btCollisionObject* m_collisionObject; + + virtual float AddSingleResult(LocalRayResult& rayResult) + { + +//caller already does the filter on the m_closestHitFraction + assert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + m_hitNormalWorld = m_collisionObject->m_worldTransform.getBasis()*rayResult.m_hitNormalLocal; + m_hitPointWorld.setInterpolate3(m_rayFromWorld,m_rayToWorld,rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; + + + + + int getNumCollisionObjects() const + { + return m_collisionObjects.size(); + } + + /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback + /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback); + + /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. + /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. + /// This allows more customization. + void rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback); + + void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); + + btCollisionObjectArray& getCollisionObjectArray() + { + return m_collisionObjects; + } + + const btCollisionObjectArray& getCollisionObjectArray() const + { + return m_collisionObjects; + } + + + void removeCollisionObject(btCollisionObject* collisionObject); + + virtual void performDiscreteCollisionDetection(); + +}; + + +#endif //COLLISION_WORLD_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp new file mode 100644 index 00000000000..febd726b556 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -0,0 +1,140 @@ +/* +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 "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" + + +btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) +:m_isSwapped(isSwapped) +{ + btCollisionObject* colObj = m_isSwapped? body1 : body0; + btCollisionObject* otherObj = m_isSwapped? body0 : body1; + assert (colObj->m_collisionShape->isCompound()); + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + int numChildren = compoundShape->getNumChildShapes(); + int i; + + m_childCollisionAlgorithms.resize(numChildren); + for (i=0;i<numChildren;i++) + { + btCollisionShape* childShape = compoundShape->getChildShape(i); + btCollisionShape* orgShape = colObj->m_collisionShape; + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj); + colObj->m_collisionShape =orgShape; + } +} + + +btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() +{ + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + delete m_childCollisionAlgorithms[i]; + } +} + +void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + btCollisionObject* colObj = m_isSwapped? body1 : body0; + btCollisionObject* otherObj = m_isSwapped? body0 : body1; + + assert (colObj->m_collisionShape->isCompound()); + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + //temporarily exchange parent btCollisionShape with childShape, and recurse + btCollisionShape* childShape = compoundShape->getChildShape(i); + + //backup + btTransform orgTrans = colObj->m_worldTransform; + btCollisionShape* orgShape = colObj->m_collisionShape; + + btTransform childTrans = compoundShape->getChildTransform(i); + btTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->m_worldTransform = newChildWorldTrans; + //the contactpoint is still projected back using the original inverted worldtrans + colObj->m_collisionShape = childShape; + m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut); + //revert back + colObj->m_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; + } +} + +float btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + btCollisionObject* colObj = m_isSwapped? body1 : body0; + btCollisionObject* otherObj = m_isSwapped? body0 : body1; + + assert (colObj->m_collisionShape->isCompound()); + + btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->m_collisionShape); + + //We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + //If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + //given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + //determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + //then use each overlapping node AABB against Tree0 + //and vise versa. + + float hitFraction = 1.f; + + int numChildren = m_childCollisionAlgorithms.size(); + int i; + for (i=0;i<numChildren;i++) + { + //temporarily exchange parent btCollisionShape with childShape, and recurse + btCollisionShape* childShape = compoundShape->getChildShape(i); + + //backup + btTransform orgTrans = colObj->m_worldTransform; + btCollisionShape* orgShape = colObj->m_collisionShape; + + btTransform childTrans = compoundShape->getChildTransform(i); + btTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->m_worldTransform = newChildWorldTrans; + + colObj->m_collisionShape = childShape; + float frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); + if (frac<hitFraction) + { + hitFraction = frac; + } + //revert back + colObj->m_collisionShape =orgShape; + colObj->m_worldTransform = orgTrans; + } + return hitFraction; + +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h new file mode 100644 index 00000000000..fe2d8628656 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -0,0 +1,64 @@ +/* +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 COMPOUND_COLLISION_ALGORITHM_H +#define COMPOUND_COLLISION_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +class btDispatcher; +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include <vector> +#include "btCollisionCreateFunc.h" + +/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes +/// Place holder, not fully implemented yet +class btCompoundCollisionAlgorithm : public btCollisionAlgorithm +{ + std::vector<btCollisionAlgorithm*> m_childCollisionAlgorithms; + bool m_isSwapped; + +public: + + btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); + + virtual ~btCompoundCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btCompoundCollisionAlgorithm(ci,body0,body1,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btCompoundCollisionAlgorithm(ci,body0,body1,true); + } + }; + +}; + +#endif //COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp new file mode 100644 index 00000000000..d7d0055f714 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -0,0 +1,303 @@ +/* +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 "btConvexConcaveCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "btConvexConvexAlgorithm.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 "LinearMath/btIDebugDraw.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" + +btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) +: btCollisionAlgorithm(ci), +m_isSwapped(isSwapped), +m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped) +{ +} + +btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() +{ +} + + + +btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): + m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) +{ + m_convexBody = 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(); +} + +btConvexTriangleCallback::~btConvexTriangleCallback() +{ + clearCache(); + m_dispatcher->releaseManifold( m_manifoldPtr ); + +} + + +void btConvexTriangleCallback::clearCache() +{ + m_dispatcher->clearManifold(m_manifoldPtr); +}; + + + +void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +{ + + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); + + + //aabb filter is already applied! + + btCollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = m_dispatcher; + + btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); + + + + ///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->m_worldTransform; + 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); + + //btVector3 center = triangle[0] + triangle[1]+triangle[2]; + //center *= 0.333333f; + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); + + } + + + //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); + + if (m_convexBody->m_collisionShape->isConvex()) + { + btTriangleShape tm(triangle[0],triangle[1],triangle[2]); + tm.setMargin(m_collisionMarginTriangle); + + + btCollisionShape* tmpShape = ob->m_collisionShape; + ob->m_collisionShape = &tm; + + ///this should use the btDispatcher, so the actual registered algorithm is used + btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); + cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + ob->m_collisionShape = tmpShape; + + } + + + +} + + + +void btConvexTriangleCallback::setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + m_dispatchInfoPtr = &dispatchInfo; + m_collisionMarginTriangle = collisionMarginTriangle; + m_resultOut = resultOut; + + //recalc aabbs + btTransform convexInTriangleSpace; + convexInTriangleSpace = m_triBody->m_worldTransform.inverse() * m_convexBody->m_worldTransform; + btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->m_collisionShape); + //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); + convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); + float extraMargin = collisionMarginTriangle; + btVector3 extra(extraMargin,extraMargin,extraMargin); + + m_aabbMax += extra; + m_aabbMin -= extra; + +} + +void btConvexConcaveCollisionAlgorithm::clearCache() +{ + m_btConvexTriangleCallback.clearCache(); + +} + +void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + + btCollisionObject* convexBody = m_isSwapped ? body1 : body0; + btCollisionObject* triBody = m_isSwapped ? body0 : body1; + + if (triBody->m_collisionShape->isConcave()) + { + + + btCollisionObject* triOb = triBody; + ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape); + + if (convexBody->m_collisionShape->isConvex()) + { + float collisionMarginTriangle = concaveShape->getMargin(); + + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.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_btConvexTriangleCallback.m_manifoldPtr); + + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); + + concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); + + + } + + } + +} + + +float btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + 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 treshold, this prevents blocking on the long run + //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2(); + if (squareMot0 < convexbody->m_ccdSquareMotionTreshold) + { + return 1.f; + } + + //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->m_worldTransform.inverse(); + btTransform convexFromLocal = triInv * convexbody->m_worldTransform; + btTransform convexToLocal = triInv * convexbody->m_interpolationWorldTransform; + + struct LocalTriangleSphereCastCallback : public btTriangleCallback + { + btTransform m_ccdSphereFromTrans; + btTransform m_ccdSphereToTrans; + btTransform m_meshTransform; + + float m_ccdSphereRadius; + float m_hitFraction; + + + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,float ccdSphereRadius,float hitFraction) + :m_ccdSphereFromTrans(from), + m_ccdSphereToTrans(to), + m_ccdSphereRadius(ccdSphereRadius), + m_hitFraction(hitFraction) + { + } + + + virtual void processTriangle(btVector3* triangle, int partId, int 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->m_collisionShape->isConcave()) + { + btVector3 rayAabbMin = convexFromLocal.getOrigin(); + rayAabbMin.setMin(convexToLocal.getOrigin()); + btVector3 rayAabbMax = convexFromLocal.getOrigin(); + rayAabbMax.setMax(convexToLocal.getOrigin()); + rayAabbMin -= btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius); + rayAabbMax += btVector3(convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius,convexbody->m_ccdSweptSphereRadius); + + float curHitFraction = 1.f; //is this available? + LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal, + convexbody->m_ccdSweptSphereRadius,curHitFraction); + + raycastCallback.m_hitFraction = convexbody->m_hitFraction; + + btCollisionObject* concavebody = triBody; + + ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape; + + if (triangleMesh) + { + triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); + } + + + + if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) + { + convexbody->m_hitFraction = raycastCallback.m_hitFraction; + return raycastCallback.m_hitFraction; + } + } + + return 1.f; + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h new file mode 100644 index 00000000000..afcb38c94ef --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -0,0 +1,111 @@ +/* +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_CONCAVE_COLLISION_ALGORITHM_H +#define CONVEX_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 "btCollisionCreateFunc.h" + +///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), processTriangle is called. +class btConvexTriangleCallback : public btTriangleCallback +{ + btCollisionObject* m_convexBody; + btCollisionObject* m_triBody; + + btVector3 m_aabbMin; + btVector3 m_aabbMax ; + + btManifoldResult* m_resultOut; + + btDispatcher* m_dispatcher; + const btDispatcherInfo* m_dispatchInfoPtr; + float m_collisionMarginTriangle; + +public: +int m_triangleCount; + + btPersistentManifold* m_manifoldPtr; + + btConvexTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); + + void setTimeStepAndCounters(float collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual ~btConvexTriangleCallback(); + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + void clearCache(); + + inline const btVector3& getAabbMin() const + { + return m_aabbMin; + } + inline const btVector3& getAabbMax() const + { + return m_aabbMax; + } + +}; + + + + +/// btConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. +class btConvexConcaveCollisionAlgorithm : public btCollisionAlgorithm +{ + + bool m_isSwapped; + + btConvexTriangleCallback m_btConvexTriangleCallback; + + +public: + + btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); + + virtual ~btConvexConcaveCollisionAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + void clearCache(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); + } + }; + + struct SwappedCreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); + } + }; + +}; + +#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp new file mode 100644 index 00000000000..54b8bc06aaa --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -0,0 +1,277 @@ +/* +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 "btConvexConvexAlgorithm.h" + +#include <stdio.h> +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" + + + +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" + +//#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" + +#ifdef WIN32 +#if _MSC_VER >= 1310 +//only use SIMD Hull code under Win32 +#ifdef TEST_HULL +#define USE_HULL 1 +#endif //TEST_HULL +#endif //_MSC_VER +#endif //WIN32 + + +#ifdef USE_HULL + +#include "NarrowPhaseCollision/Hull.h" +#include "NarrowPhaseCollision/HullContactCollector.h" + + +#endif //USE_HULL + +bool gUseEpa = false; + + +#ifdef WIN32 +void DrawRasterizerLine(const float* from,const float* to,int color); +#endif + + + + +//#define PROCESS_SINGLE_CONTACT +#ifdef WIN32 +bool gForceBoxBox = false;//false;//true; + +#else +bool gForceBoxBox = false;//false;//true; +#endif +bool gBoxBoxUseGjk = true;//true;//false; +bool gDisableConvexCollision = false; + + + +btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1) +: btCollisionAlgorithm(ci), +m_gjkPairDetector(0,0,&m_simplexSolver,0), +m_useEpa(!gUseEpa), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false) +{ + checkPenetrationDepthSolver(); + +} + + + + +btConvexConvexAlgorithm::~btConvexConvexAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + + + +static btMinkowskiPenetrationDepthSolver gPenetrationDepthSolver; + +//static EpaPenetrationDepthSolver gEpaPenetrationDepthSolver; + +#ifdef USE_EPA +Solid3EpaPenetrationDepth gSolidEpaPenetrationSolver; +#endif //USE_EPA + +void btConvexConvexAlgorithm::checkPenetrationDepthSolver() +{ + if (m_useEpa != gUseEpa) + { + m_useEpa = gUseEpa; + if (m_useEpa) + { + + // m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver); + + + } else + { + m_gjkPairDetector.setPenetrationDepthSolver(&gPenetrationDepthSolver); + } + } + +} + + +// +// Convex-Convex collision algorithm +// +void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_ownManifold = true; + } + + + checkPenetrationDepthSolver(); + + btConvexShape* min0 = static_cast<btConvexShape*>(body0->m_collisionShape); + btConvexShape* min1 = static_cast<btConvexShape*>(body1->m_collisionShape); + + btGjkPairDetector::ClosestPointInput input; + + //TODO: if (dispatchInfo.m_useContinuous) + m_gjkPairDetector.setMinkowskiA(min0); + m_gjkPairDetector.setMinkowskiB(min1); + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingTreshold(); + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + +// input.m_maximumDistanceSquared = 1e30f; + + input.m_transformA = body0->m_worldTransform; + input.m_transformB = body1->m_worldTransform; + + resultOut->setPersistentManifold(m_manifoldPtr); + m_gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + +} + + + +bool disableCcd = false; +float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds treshold + + ///Linear motion for one of objects needs to exceed m_ccdSquareMotionTreshold + ///col0->m_worldTransform, + float resultFraction = 1.f; + + + float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); + + if (squareMot0 < col0->m_ccdSquareMotionTreshold && + squareMot0 < col0->m_ccdSquareMotionTreshold) + return resultFraction; + + + + if (disableCcd) + return 1.f; + + checkPenetrationDepthSolver(); + + //An adhoc way of testing the Continuous Collision Detection algorithms + //One object is approximated as a sphere, to simplify things + //Starting in penetration should report no time of impact + //For proper CCD, better accuracy and handling of 'allowed' penetration should be added + //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + + /// Convex0 against sphere for Convex1 + { + btConvexShape* convex0 = static_cast<btConvexShape*>(col0->m_collisionShape); + + btSphereShape sphere1(col1->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, + col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + { + + //store result.m_fraction in both bodies + + if (col0->m_hitFraction > result.m_fraction) + col0->m_hitFraction = result.m_fraction; + + if (col1->m_hitFraction > result.m_fraction) + col1->m_hitFraction = result.m_fraction; + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + + + + + } + + /// Sphere (for convex0) against Convex1 + { + btConvexShape* convex1 = static_cast<btConvexShape*>(col1->m_collisionShape); + + btSphereShape sphere0(col0->m_ccdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform, + col1->m_worldTransform,col1->m_interpolationWorldTransform,result)) + { + + //store result.m_fraction in both bodies + + if (col0->m_hitFraction > result.m_fraction) + col0->m_hitFraction = result.m_fraction; + + if (col1->m_hitFraction > result.m_fraction) + col1->m_hitFraction = result.m_fraction; + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + } + + return resultFraction; + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h new file mode 100644 index 00000000000..b8e121714ad --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -0,0 +1,81 @@ +/* +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_CONVEX_ALGORITHM_H +#define CONVEX_CONVEX_ALGORITHM_H + +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "btCollisionCreateFunc.h" + +class btConvexPenetrationDepthSolver; + +///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. +class btConvexConvexAlgorithm : public btCollisionAlgorithm +{ + //ConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btVoronoiSimplexSolver m_simplexSolver; + btGjkPairDetector m_gjkPairDetector; + bool m_useEpa; +public: + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + void checkPenetrationDepthSolver(); + + + +public: + + btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btConvexConvexAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + void setLowLevelOfDetail(bool useLowLevel); + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + m_gjkPairDetector.m_partId0=partId0; + m_gjkPairDetector.m_partId1=partId1; + m_gjkPairDetector.m_index0=index0; + m_gjkPairDetector.m_index1=index1; + } + + const btPersistentManifold* getManifold() + { + return m_manifoldPtr; + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btConvexConvexAlgorithm(0,ci,body0,body1); + } + }; + + +}; + +#endif //CONVEX_CONVEX_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp new file mode 100644 index 00000000000..9bc106564af --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp @@ -0,0 +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 "btEmptyCollisionAlgorithm.h" + + + +btEmptyAlgorithm::btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) +{ +} + +void btEmptyAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + +} + +float btEmptyAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + return 1.f; +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h new file mode 100644 index 00000000000..e0e136250ac --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef EMPTY_ALGORITH +#define EMPTY_ALGORITH +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "btCollisionCreateFunc.h" + +#define ATTRIBUTE_ALIGNED(a) + +///EmptyAlgorithm is a stub for unsupported collision pairs. +///The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame. +class btEmptyAlgorithm : public btCollisionAlgorithm +{ + +public: + + btEmptyAlgorithm(const btCollisionAlgorithmConstructionInfo& ci); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btEmptyAlgorithm(ci); + } + }; + +} ATTRIBUTE_ALIGNED(16); + +#endif //EMPTY_ALGORITH diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp new file mode 100644 index 00000000000..7031521f66b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -0,0 +1,107 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btManifoldResult.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + + +///This is to allow MaterialCombiner/Custom Friction/Restitution values +ContactAddedCallback gContactAddedCallback=0; + +///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; +inline btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1) +{ + btScalar friction = body0->getFriction() * body1->getFriction(); + + const btScalar MAX_FRICTION = 10.f; + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + +inline btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1) +{ + return body0->getRestitution() * body1->getRestitution(); +} + + + +btManifoldResult::btManifoldResult(btCollisionObject* body0,btCollisionObject* body1) + :m_manifoldPtr(0), + m_body0(body0), + m_body1(body1) +{ + m_rootTransA = body0->m_worldTransform; + m_rootTransB = body1->m_worldTransform; +} + + +void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) +{ + assert(m_manifoldPtr); + //order in manifold needs to match + + if (depth > m_manifoldPtr->getContactBreakingTreshold()) + return; + + bool isSwapped = m_manifoldPtr->getBody0() != m_body0; + + btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse(); + btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse(); + + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + btVector3 localA = transAInv(pointA ); + btVector3 localB = transBInv(pointInWorld); + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + + + int insertIndex = m_manifoldPtr->getCacheEntry(newPt); + if (insertIndex >= 0) + { + +// This is not needed, just use the old info! +// const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); +// newPt.CopyPersistentInformation(oldPoint); +// m_manifoldPtr->replaceContactPoint(newPt,insertIndex); + + + } else + { + + newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); + newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); + + //User can override friction and/or restitution + if (gContactAddedCallback && + //and if either of the two bodies requires custom material + ((m_body0->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK) || + (m_body1->m_collisionFlags & btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK))) + { + //experimental feature info, for per-triangle material etc. + btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; + btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; + (*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); + } + + m_manifoldPtr->AddManifoldPoint(newPt); + } +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h new file mode 100644 index 00000000000..f9aae4e9c9a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -0,0 +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 MANIFOLD_RESULT_H +#define MANIFOLD_RESULT_H + +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +struct btCollisionObject; +class btPersistentManifold; +class btManifoldPoint; +#include "LinearMath/btTransform.h" + +typedef bool (*ContactAddedCallback)(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1); +extern ContactAddedCallback gContactAddedCallback; + + + +///btManifoldResult is a helper class to manage contact results. +class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result +{ + btPersistentManifold* m_manifoldPtr; + + //we need this for compounds + btTransform m_rootTransA; + btTransform m_rootTransB; + + btCollisionObject* m_body0; + btCollisionObject* m_body1; + int m_partId0; + int m_partId1; + int m_index0; + int m_index1; +public: + + btManifoldResult() + { + } + + btManifoldResult(btCollisionObject* body0,btCollisionObject* body1); + + virtual ~btManifoldResult() {}; + + void setPersistentManifold(btPersistentManifold* manifoldPtr) + { + m_manifoldPtr = manifoldPtr; + } + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + m_partId0=partId0; + m_partId1=partId1; + m_index0=index0; + m_index1=index1; + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth); + + + +}; + +#endif //MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp new file mode 100644 index 00000000000..fa52d7e055a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -0,0 +1,280 @@ + +#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 <algorithm> + + +btSimulationIslandManager::btSimulationIslandManager() +{ +} + +btSimulationIslandManager::~btSimulationIslandManager() +{ +} + + +void btSimulationIslandManager::initUnionFind(int n) +{ + m_unionFind.reset(n); +} + + +void btSimulationIslandManager::findUnions(btDispatcher* dispatcher) +{ + + { + for (int i=0;i<dispatcher->getNumManifolds();i++) + { + const btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + //static objects (invmass 0.f) don't merge ! + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + + m_unionFind.unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + } +} + + +void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher) +{ + + initUnionFind(colWorld->getCollisionObjectArray().size()); + + // put the index into m_controllers into m_tag + { + std::vector<btCollisionObject*>::iterator i; + + int index = 0; + for (i=colWorld->getCollisionObjectArray().begin(); + !(i==colWorld->getCollisionObjectArray().end()); i++) + { + + btCollisionObject* collisionObject= (*i); + collisionObject->m_islandTag1 = index; + collisionObject->m_hitFraction = 1.f; + index++; + + } + } + // do the union find + + findUnions(dispatcher); + + + +} + + + + +void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld) +{ + // put the islandId ('find' value) into m_tag + { + + + std::vector<btCollisionObject*>::iterator i; + + int index = 0; + for (i=colWorld->getCollisionObjectArray().begin(); + !(i==colWorld->getCollisionObjectArray().end()); i++) + { + btCollisionObject* collisionObject= (*i); + + if (collisionObject->mergesSimulationIslands()) + { + collisionObject->m_islandTag1 = m_unionFind.find(index); + } else + { + collisionObject->m_islandTag1 = -1; + } + 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->m_islandTag1>=0?rcolObj0->m_islandTag1:rcolObj1->m_islandTag1; + return islandId; + +} + +bool btPersistentManifoldSortPredicate(const btPersistentManifold* lhs, const btPersistentManifold* rhs) +{ + int rIslandId0,lIslandId0; + rIslandId0 = getIslandId(rhs); + lIslandId0 = getIslandId(lhs); + return lIslandId0 < rIslandId0; +} + + +// +// todo: this is random access, it can be walked 'cache friendly'! +// +void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) +{ + //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; + + //update the sleeping state for bodies, if all are sleeping + for (int 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->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + { + printf("error in island management\n"); + } + + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + if (colObj0->m_islandTag1 == 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->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + { + printf("error in island management\n"); + } + + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + + if (colObj0->m_islandTag1 == 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->m_islandTag1 != islandId) && (colObj0->m_islandTag1 != -1)) + { + printf("error in island management\n"); + } + + assert((colObj0->m_islandTag1 == islandId) || (colObj0->m_islandTag1 == -1)); + + if (colObj0->m_islandTag1 == islandId) + { + if ( colObj0->GetActivationState() == ISLAND_SLEEPING) + { + colObj0->SetActivationState( WANTS_DEACTIVATION); + } + } + } + } + } + + std::vector<btPersistentManifold*> islandmanifold; + int i; + int maxNumManifolds = dispatcher->getNumManifolds(); + islandmanifold.reserve(maxNumManifolds); + + 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->SetActivationState(ACTIVE_TAG); + } + if (colObj1->isKinematicObject() && colObj1->GetActivationState() != ISLAND_SLEEPING) + { + colObj0->SetActivationState(ACTIVE_TAG); + } + + //filtering for response + if (dispatcher->needsResponse(colObj0,colObj1)) + islandmanifold.push_back(manifold); + } + } + + int numManifolds = islandmanifold.size(); + + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + //now process all active islands (sets of manifolds for now) + + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + for (startManifoldIndex=0;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex) + { + int islandId = getIslandId(islandmanifold[startManifoldIndex]); + for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++) + { + } + /// Process the actual simulation, only if not sleeping/deactivated + int numIslandManifolds = endManifoldIndex-startManifoldIndex; + if (numIslandManifolds) + { + callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds); + } + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h new file mode 100644 index 00000000000..36c51000a6b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -0,0 +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 SIMULATION_ISLAND_MANAGER_H +#define SIMULATION_ISLAND_MANAGER_H + +#include "BulletCollision/CollisionDispatch/btUnionFind.h" +#include "btCollisionCreateFunc.h" + +class btCollisionWorld; +class btDispatcher; + +///SimulationIslandManager creates and handles simulation islands, using btUnionFind +class btSimulationIslandManager +{ + btUnionFind m_unionFind; + +public: + btSimulationIslandManager(); + virtual ~btSimulationIslandManager(); + + + void initUnionFind(int n); + + + btUnionFind& getUnionFind() { return m_unionFind;} + + virtual void updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher); + virtual void storeIslandActivationState(btCollisionWorld* world); + + + void findUnions(btDispatcher* dispatcher); + + + + struct IslandCallback + { + virtual ~IslandCallback() {}; + + virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds) = 0; + }; + + void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); + +}; + +#endif //SIMULATION_ISLAND_MANAGER_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp new file mode 100644 index 00000000000..d32818444d7 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp @@ -0,0 +1,242 @@ +/* +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) +: btCollisionAlgorithm(ci), +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) +{ + if (!m_manifoldPtr) + return; + + btCollisionObject* sphereObj = m_isSwapped? body1 : body0; + btCollisionObject* boxObj = m_isSwapped? body0 : body1; + + + btSphereShape* sphere0 = (btSphereShape*)sphereObj ->m_collisionShape; + + btVector3 normalOnSurfaceB; + btVector3 pOnBox,pOnSphere; + btVector3 sphereCenter = sphereObj->m_worldTransform.getOrigin(); + btScalar radius = sphere0->getRadius(); + + float dist = getSphereDistance(boxObj,pOnBox,pOnSphere,sphereCenter,radius); + + if (dist < SIMD_EPSILON) + { + btVector3 normalOnSurfaceB = (pOnBox- pOnSphere).normalize(); + + /// report a contact. internally this will be kept persistent, and contact reduction is done + + resultOut->setPersistentManifold(m_manifoldPtr); + resultOut->addContactPoint(normalOnSurfaceB,pOnBox,dist); + + } + + + +} + +float btSphereBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + //not yet + return 1.f; +} + + +btScalar btSphereBoxCollisionAlgorithm::getSphereDistance(btCollisionObject* boxObj, btVector3& pointOnBox, btVector3& v3PointOnSphere, const btVector3& sphereCenter, btScalar fRadius ) +{ + + btScalar margins; + btVector3 bounds[2]; + btBoxShape* boxShape= (btBoxShape*)boxObj->m_collisionShape; + + bounds[0] = -boxShape->getHalfExtents(); + bounds[1] = boxShape->getHalfExtents(); + + margins = boxShape->getMargin();//also add sphereShape margin? + + const btTransform& m44T = boxObj->m_worldTransform; + + 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 = 10000000.0f, fSepThis; + + n[0].setValue( -1.0f, 0.0f, 0.0f ); + n[1].setValue( 0.0f, -1.0f, 0.0f ); + n[2].setValue( 0.0f, 0.0f, -1.0f ); + n[3].setValue( 1.0f, 0.0f, 0.0f ); + n[4].setValue( 0.0f, 1.0f, 0.0f ); + n[5].setValue( 0.0f, 0.0f, 1.0f ); + + // 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]))) > 0.0f ) + { + 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)) > 0.0f ) + { + return 1.0f; + } + + // 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 *= 1.f/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 <= 0.0f ) + return (fPenetration-margins); + else + return 1.0f; +} + +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 = -10000000.0f, fSepThis; + + n[0].setValue( -1.0f, 0.0f, 0.0f ); + n[1].setValue( 0.0f, -1.0f, 0.0f ); + n[2].setValue( 0.0f, 0.0f, -1.0f ); + n[3].setValue( 1.0f, 0.0f, 0.0f ); + n[4].setValue( 0.0f, 1.0f, 0.0f ); + n[5].setValue( 0.0f, 0.0f, 1.0f ); + + const btTransform& m44T = boxObj->m_worldTransform; + + // 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) > 0.0f ) return 1.0f; + if ( fSepThis > fSep ) + { + p0 = bounds[j]; normal = (btVector3&)n[i]; + fSep = fSepThis; + } + } + + pointOnBox = prel - normal*(normal.dot((prel-p0))); + v3PointOnSphere = pointOnBox + normal*fSep; + + // transform back in world space + tmp = m44T( pointOnBox); + pointOnBox = tmp; + tmp = m44T( v3PointOnSphere); v3PointOnSphere = tmp; + normal = (pointOnBox-v3PointOnSphere).normalize(); + + return fSep; + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h new file mode 100644 index 00000000000..68915a43e6f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -0,0 +1,64 @@ +/* +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 "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +class btPersistentManifold; +#include "LinearMath/btVector3.h" + +/// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. +/// Other features are frame-coherency (persistent data) and collision response. +class btSphereBoxCollisionAlgorithm : public btCollisionAlgorithm +{ + 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 float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + 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) + { + if (!m_swapped) + { + return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); + } else + { + return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); + } + } + }; + +}; + +#endif //SPHERE_BOX_COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp new file mode 100644 index 00000000000..bb04833ca5a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp @@ -0,0 +1,78 @@ +/* +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) +: btCollisionAlgorithm(ci), +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) +{ + if (!m_manifoldPtr) + return; + + btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape; + btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape; + + btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin(); + float len = diff.length(); + btScalar radius0 = sphere0->getRadius(); + btScalar radius1 = sphere1->getRadius(); + + ///iff distance positive, don't generate a new contact + if ( len > (radius0+radius1)) + return; + + ///distance (negative means penetration) + btScalar dist = len - (radius0+radius1); + + btVector3 normalOnSurfaceB = diff / len; + ///point on A (worldspace) + btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB; + ///point on B (worldspace) + btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB; + + /// report a contact. internally this will be kept persistent, and contact reduction is done + resultOut->setPersistentManifold(m_manifoldPtr); + resultOut->addContactPoint(normalOnSurfaceB,pos1,dist); + +} + +float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + //not yet + return 1.f; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h new file mode 100644 index 00000000000..8b08d015aec --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -0,0 +1,56 @@ +/* +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 "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.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 btCollisionAlgorithm +{ + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + +public: + btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1); + + btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) + : btCollisionAlgorithm(ci) {} + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + + virtual ~btSphereSphereCollisionAlgorithm(); + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1); + } + }; + +}; + +#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp new file mode 100644 index 00000000000..046f348a147 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -0,0 +1,77 @@ +/* +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 "btUnionFind.h" +#include <assert.h> +#include <algorithm> + + + +btUnionFind::~btUnionFind() +{ + Free(); + +} + +btUnionFind::btUnionFind() +{ + +} + +void btUnionFind::allocate(int N) +{ + m_elements.resize(N); +} +void btUnionFind::Free() +{ + m_elements.clear(); +} + + +void btUnionFind::reset(int N) +{ + allocate(N); + + for (int i = 0; i < N; i++) + { + m_elements[i].m_id = i; m_elements[i].m_sz = 1; + } +} + +bool btUnionFindElementSortPredicate(const btElement& lhs, const btElement& rhs) +{ + return lhs.m_id < rhs.m_id; +} + + +///this is a special operation, destroying the content of btUnionFind. +///it sorts the elements, based on island id, in order to make it easy to iterate over islands +void btUnionFind::sortIslands() +{ + + //first store the original body index, and islandId + int numElements = m_elements.size(); + + for (int i=0;i<numElements;i++) + { + m_elements[i].m_id = find(i); + m_elements[i].m_sz = i; + } + + // Sort the vector using predicate and std::sort + std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h new file mode 100644 index 00000000000..8db1580e41e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -0,0 +1,117 @@ +/* +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 UNION_FIND_H +#define UNION_FIND_H + +#include <vector> +struct btElement +{ + int m_id; + int m_sz; +}; + +///UnionFind calculates connected subsets +// Implements weighted Quick Union with path compression +// optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable) +class btUnionFind + { + private: + std::vector<btElement> m_elements; + + public: + + btUnionFind(); + ~btUnionFind(); + + + //this is a special operation, destroying the content of btUnionFind. + //it sorts the elements, based on island id, in order to make it easy to iterate over islands + void sortIslands(); + + void reset(int N); + + inline int getNumElements() const + { + return m_elements.size(); + } + inline bool isRoot(int x) const + { + return (x == m_elements[x].m_id); + } + + btElement& getElement(int index) + { + return m_elements[index]; + } + const btElement& getElement(int index) const + { + return m_elements[index]; + } + + void allocate(int N); + void Free(); + + + + + int find(int p, int q) + { + return (find(p) == find(q)); + } + + void unite(int p, int q) + { + int i = find(p), j = find(q); + if (i == j) + return; + + //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + if (m_elements[i].m_sz < m_elements[j].m_sz) + { + m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; + } + else + { + m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; + } + } + + int find(int x) + { + //assert(x < m_N); + //assert(x >= 0); + + while (x != m_elements[x].m_id) + { + //not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + #define USE_PATH_COMPRESSION 1 + #ifdef USE_PATH_COMPRESSION + // + m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id; + #endif // + x = m_elements[x].m_id; + //assert(x < m_N); + //assert(x >= 0); + + } + return x; + } + + + }; + + +#endif //UNION_FIND_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp new file mode 100644 index 00000000000..b5f80de4557 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -0,0 +1,58 @@ +/* +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 "btBoxShape.h" + +btVector3 btBoxShape::getHalfExtents() const +{ + return m_implicitShapeDimensions * m_localScaling; +} +//{ + + +void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 halfExtents = getHalfExtents(); + + btMatrix3x3 abs_b = t.getBasis().absolute(); + btPoint3 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; + + +} + + +void btBoxShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //float margin = 0.f; + btVector3 halfExtents = getHalfExtents(); + + btScalar lx=2.f*(halfExtents.x()); + btScalar ly=2.f*(halfExtents.y()); + btScalar lz=2.f*(halfExtents.z()); + + inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); + inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); + inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h new file mode 100644 index 00000000000..b137eb1150e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -0,0 +1,262 @@ +/* +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 OBB_BOX_MINKOWSKI_H +#define OBB_BOX_MINKOWSKI_H + +#include "btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "LinearMath/btPoint3.h" +#include "LinearMath/btSimdMinMax.h" + +///btBoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box +class btBoxShape: public btPolyhedralConvexShape +{ + + //btVector3 m_boxHalfExtents1; //use m_implicitShapeDimensions instead + + +public: + + btVector3 getHalfExtents() const; + + virtual int getShapeType() const { return BOX_SHAPE_PROXYTYPE;} + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + { + + btVector3 halfExtents = getHalfExtents(); + + btVector3 supVertex; + supVertex = btPoint3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + + return supVertex; + } + + virtual inline btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + { + btVector3 halfExtents = getHalfExtents(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents -= margin; + + return btVector3(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + } + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + { + btVector3 halfExtents = getHalfExtents(); + btVector3 margin(getMargin(),getMargin(),getMargin()); + halfExtents -= margin; + + + for (int i=0;i<numVectors;i++) + { + const btVector3& vec = vectors[i]; + supportVerticesOut[i].setValue(vec.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + } + + } + + + btBoxShape( const btVector3& boxHalfExtents) + { + m_implicitShapeDimensions = boxHalfExtents; + }; + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const + { + //this plane might not be aligned... + btVector4 plane ; + getPlaneEquation(plane,i); + planeNormal = btVector3(plane.getX(),plane.getY(),plane.getZ()); + planeSupport = localGetSupportingVertex(-planeNormal); + } + + + virtual int getNumPlanes() const + { + return 6; + } + + virtual int getNumVertices() const + { + return 8; + } + + virtual int getNumEdges() const + { + return 12; + } + + + virtual void getVertex(int i,btVector3& vtx) const + { + btVector3 halfExtents = getHalfExtents(); + + vtx = btVector3( + halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), + halfExtents.y() * (1-((i&2)>>1)) - halfExtents.y() * ((i&2)>>1), + halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + } + + + virtual void getPlaneEquation(btVector4& plane,int i) const + { + btVector3 halfExtents = getHalfExtents(); + + switch (i) + { + case 0: + plane.setValue(1.f,0.f,0.f); + plane[3] = -halfExtents.x(); + break; + case 1: + plane.setValue(-1.f,0.f,0.f); + plane[3] = -halfExtents.x(); + break; + case 2: + plane.setValue(0.f,1.f,0.f); + plane[3] = -halfExtents.y(); + break; + case 3: + plane.setValue(0.f,-1.f,0.f); + plane[3] = -halfExtents.y(); + break; + case 4: + plane.setValue(0.f,0.f,1.f); + plane[3] = -halfExtents.z(); + break; + case 5: + plane.setValue(0.f,0.f,-1.f); + plane[3] = -halfExtents.z(); + break; + default: + assert(0); + } + } + + + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + //virtual void getEdge(int i,Edge& edge) const + { + int edgeVert0 = 0; + int edgeVert1 = 0; + + switch (i) + { + case 0: + edgeVert0 = 0; + edgeVert1 = 1; + break; + case 1: + edgeVert0 = 0; + edgeVert1 = 2; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + ASSERT(0); + + } + + getVertex(edgeVert0,pa ); + getVertex(edgeVert1,pb ); + } + + + + + + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + { + btVector3 halfExtents = getHalfExtents(); + + //btScalar minDist = 2*tolerance; + + bool result = (pt.x() <= (halfExtents.x()+tolerance)) && + (pt.x() >= (-halfExtents.x()-tolerance)) && + (pt.y() <= (halfExtents.y()+tolerance)) && + (pt.y() >= (-halfExtents.y()-tolerance)) && + (pt.z() <= (halfExtents.z()+tolerance)) && + (pt.z() >= (-halfExtents.z()-tolerance)); + + return result; + } + + + //debugging + virtual char* getName()const + { + return "Box"; + } + + +}; + +#endif //OBB_BOX_MINKOWSKI_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp new file mode 100644 index 00000000000..338527d58a4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -0,0 +1,138 @@ +/* +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. +*/ + +//#define DISABLE_BVH + + +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + +///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. +///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. +btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface) +:btTriangleMeshShape(meshInterface) +{ + //construct bvh from meshInterface +#ifndef DISABLE_BVH + + m_bvh = new btOptimizedBvh(); + m_bvh->build(meshInterface); + +#endif //DISABLE_BVH + +} + +btBvhTriangleMeshShape::~btBvhTriangleMeshShape() +{ + delete m_bvh; +} + +//perform bvh tree traversal and report overlapping triangles to 'callback' +void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + +#ifdef DISABLE_BVH + //brute force traverse all triangles + btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); +#else + + //first get all the nodes + + + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleCallback* m_callback; + btVector3 m_triangle[3]; + + + MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(const btOptimizedBvhNode* node) + { + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + node->m_subPart); + + int* gfxbase = (int*)(indexbase+node->m_triangleIndex*indexstride); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + + int graphicsindex = gfxbase[j]; +#ifdef DEBUG_TRIANGLE_MESH + printf("%d ,",graphicsindex); +#endif //DEBUG_TRIANGLE_MESH + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3( + graphicsbase[0]*meshScaling.getX(), + graphicsbase[1]*meshScaling.getY(), + graphicsbase[2]*meshScaling.getZ()); +#ifdef DEBUG_TRIANGLE_MESH + printf("triangle vertices:%f,%f,%f\n",triangle[j].x(),triangle[j].y(),triangle[j].z()); +#endif //DEBUG_TRIANGLE_MESH + } + + m_callback->processTriangle(m_triangle,node->m_subPart,node->m_triangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(node->m_subPart); + } + + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax); + + +#endif//DISABLE_BVH + + +} + + +void btBvhTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) + { + btTriangleMeshShape::setLocalScaling(scaling); + delete m_bvh; + m_bvh = new btOptimizedBvh(); + m_bvh->build(m_meshInterface); + //rebuild the bvh... + } +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h new file mode 100644 index 00000000000..59a27e8641a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -0,0 +1,58 @@ +/* +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 BVH_TRIANGLE_MESH_SHAPE_H +#define BVH_TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btOptimizedBvh.h" + +///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. +///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. +class btBvhTriangleMeshShape : public btTriangleMeshShape +{ + + btOptimizedBvh* m_bvh; + + +public: + btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface); + + virtual ~btBvhTriangleMeshShape(); + + + /* + virtual int getShapeType() const + { + return TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + */ + + + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + + //debugging + virtual char* getName()const {return "BVHTRIANGLEMESH";} + + + virtual void setLocalScaling(const btVector3& scaling); + + + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h new file mode 100644 index 00000000000..377f0e506a2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -0,0 +1,26 @@ +/* +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 COLLISION_MARGIN_H +#define COLLISION_MARGIN_H + +//used by Gjk and some other algorithms + +#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f + + + +#endif //COLLISION_MARGIN_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp new file mode 100644 index 00000000000..5474a201c37 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -0,0 +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. +*/ + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" + +void btCollisionShape::getBoundingSphere(btVector3& center,btScalar& radius) const +{ + btTransform tr; + tr.setIdentity(); + btVector3 aabbMin,aabbMax; + + getAabb(tr,aabbMin,aabbMax); + + radius = (aabbMax-aabbMin).length()*0.5f; + center = (aabbMin+aabbMax)*0.5f; +} + +float btCollisionShape::getAngularMotionDisc() const +{ + btVector3 center; + float disc; + getBoundingSphere(center,disc); + disc += (center).length(); + return disc; +} + +void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) +{ + //start with static aabb + getAabb(curTrans,temporalAabbMin,temporalAabbMax); + + float temporalAabbMaxx = temporalAabbMax.getX(); + float temporalAabbMaxy = temporalAabbMax.getY(); + float temporalAabbMaxz = temporalAabbMax.getZ(); + float temporalAabbMinx = temporalAabbMin.getX(); + float temporalAabbMiny = temporalAabbMin.getY(); + float temporalAabbMinz = temporalAabbMin.getZ(); + + // add linear motion + btVector3 linMotion = linvel*timeStep; + //todo: simd would have a vector max/min operation, instead of per-element access + if (linMotion.x() > 0.f) + temporalAabbMaxx += linMotion.x(); + else + temporalAabbMinx += linMotion.x(); + if (linMotion.y() > 0.f) + temporalAabbMaxy += linMotion.y(); + else + temporalAabbMiny += linMotion.y(); + if (linMotion.z() > 0.f) + temporalAabbMaxz += linMotion.z(); + else + temporalAabbMinz += linMotion.z(); + + //add conservative angular motion + btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; + btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h new file mode 100644 index 00000000000..d015fb2baae --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -0,0 +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. +*/ + +#ifndef COLLISION_SHAPE_H +#define COLLISION_SHAPE_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" +#include <LinearMath/btMatrix3x3.h> +#include "LinearMath/btPoint3.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for the shape types + +///btCollisionShape provides interface for collision shapes that can be shared among btCollisionObjects. +class btCollisionShape +{ +public: + + btCollisionShape() :m_tempDebug(0) + { + } + virtual ~btCollisionShape() + { + } + + ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const =0; + + virtual void getBoundingSphere(btVector3& center,btScalar& radius) const; + + ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. + virtual float getAngularMotionDisc() const; + + virtual int getShapeType() const=0; + + ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) + ///result is conservative + void calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax); + + inline bool isPolyhedral() const + { + return btBroadphaseProxy::isPolyhedral(getShapeType()); + } + + inline bool isConvex() const + { + return btBroadphaseProxy::isConvex(getShapeType()); + } + inline bool isConcave() const + { + return btBroadphaseProxy::isConcave(getShapeType()); + } + inline bool isCompound() const + { + return btBroadphaseProxy::isCompound(getShapeType()); + } + + virtual void setLocalScaling(const btVector3& scaling) =0; + virtual const btVector3& getLocalScaling() const =0; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) = 0; + +//debugging support + virtual char* getName()const =0 ; + const char* getExtraDebugInfo() const { return m_tempDebug;} + void setExtraDebugInfo(const char* extraDebugInfo) { m_tempDebug = extraDebugInfo;} + const char * m_tempDebug; +//endif debugging support + + virtual void setMargin(float margin) = 0; + virtual float getMargin() const = 0; + +}; + +#endif //COLLISION_SHAPE_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp new file mode 100644 index 00000000000..88ae8c7dfd4 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -0,0 +1,100 @@ +/* +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 "btCompoundShape.h" + + +#include "btCollisionShape.h" + + +btCompoundShape::btCompoundShape() +:m_localAabbMin(1e30f,1e30f,1e30f), +m_localAabbMax(-1e30f,-1e30f,-1e30f), +m_aabbTree(0), +m_collisionMargin(0.f), +m_localScaling(1.f,1.f,1.f) +{ +} + + +btCompoundShape::~btCompoundShape() +{ +} + +void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape) +{ + m_childTransforms.push_back(localTransform); + m_childShapes.push_back(shape); + + //extend the local aabbMin/aabbMax + btVector3 localAabbMin,localAabbMax; + shape->getAabb(localTransform,localAabbMin,localAabbMax); + for (int i=0;i<3;i++) + { + if (m_localAabbMin[i] > localAabbMin[i]) + { + m_localAabbMin[i] = localAabbMin[i]; + } + if (m_localAabbMax[i] < localAabbMax[i]) + { + m_localAabbMax[i] = localAabbMax[i]; + } + + } +} + + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += btVector3(getMargin(),getMargin(),getMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; +} + +void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //approximation: take the inertia from the aabb for now + btTransform ident; + ident.setIdentity(); + btVector3 aabbMin,aabbMax; + getAabb(ident,aabbMin,aabbMax); + + btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + btScalar lx=2.f*(halfExtents.x()); + btScalar ly=2.f*(halfExtents.y()); + btScalar lz=2.f*(halfExtents.z()); + + inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); + inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); + inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h new file mode 100644 index 00000000000..65a6809d4ff --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -0,0 +1,117 @@ +/* +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 COMPOUND_SHAPE_H +#define COMPOUND_SHAPE_H + +#include "btCollisionShape.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" +#include <vector> +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +class btOptimizedBvh; + +/// btCompoundShape allows to store multiple other btCollisionShapes +/// This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape. +class btCompoundShape : public btCollisionShape +{ + std::vector<btTransform> m_childTransforms; + std::vector<btCollisionShape*> m_childShapes; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + btOptimizedBvh* m_aabbTree; + +public: + btCompoundShape(); + + virtual ~btCompoundShape(); + + void addChildShape(const btTransform& localTransform,btCollisionShape* shape); + + int getNumChildShapes() const + { + return m_childShapes.size(); + } + + btCollisionShape* getChildShape(int index) + { + return m_childShapes[index]; + } + const btCollisionShape* getChildShape(int index) const + { + return m_childShapes[index]; + } + + btTransform getChildTransform(int index) + { + return m_childTransforms[index]; + } + const btTransform getChildTransform(int index) const + { + return m_childTransforms[index]; + } + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + virtual void setLocalScaling(const btVector3& scaling) + { + m_localScaling = scaling; + } + virtual const btVector3& getLocalScaling() const + { + return m_localScaling; + } + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + virtual int getShapeType() const { return COMPOUND_SHAPE_PROXYTYPE;} + + virtual void setMargin(float margin) + { + m_collisionMargin = margin; + } + virtual float getMargin() const + { + return m_collisionMargin; + } + virtual char* getName()const + { + return "Compound"; + } + + //this is optional, but should make collision queries faster, by culling non-overlapping nodes + void createAabbTreeFromChildren(); + + const btOptimizedBvh* getAabbTree() const + { + return m_aabbTree; + } + +private: + btScalar m_collisionMargin; +protected: + btVector3 m_localScaling; + +}; + + + +#endif //COMPOUND_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp new file mode 100644 index 00000000000..29f62828d04 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.cpp @@ -0,0 +1,28 @@ + +/* +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 "btConcaveShape.h" + +ConcaveShape::ConcaveShape() : m_collisionMargin(0.f) +{ + +} + +ConcaveShape::~ConcaveShape() +{ + +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h new file mode 100644 index 00000000000..304654531ed --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -0,0 +1,51 @@ +/* +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 CONCAVE_SHAPE_H +#define CONCAVE_SHAPE_H + +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +#include "btTriangleCallback.h" + + +///Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB. +///Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface. +class ConcaveShape : public btCollisionShape +{ +protected: + float m_collisionMargin; + +public: + ConcaveShape(); + + virtual ~ConcaveShape(); + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const = 0; + + virtual float getMargin() const { + return m_collisionMargin; + } + virtual void setMargin(float collisionMargin) + { + m_collisionMargin = collisionMargin; + } + + + +}; + +#endif //CONCAVE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp new file mode 100644 index 00000000000..13875fc5fe6 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.cpp @@ -0,0 +1,100 @@ +/* +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 "btConeShape.h" +#include "LinearMath/btPoint3.h" + +#ifdef WIN32 +static int coneindices[3] = {1,2,0}; +#else +static int coneindices[3] = {2,1,0}; +#endif + +btConeShape::btConeShape (btScalar radius,btScalar height): +m_radius (radius), +m_height(height) +{ + btVector3 halfExtents; + m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height)); +} + + +btVector3 btConeShape::coneLocalSupport(const btVector3& v) const +{ + + float halfHeight = m_height * 0.5f; + + if (v[coneindices[1]] > v.length() * m_sinAngle) + { + btVector3 tmp; + + tmp[coneindices[0]] = 0.f; + tmp[coneindices[1]] = halfHeight; + tmp[coneindices[2]] = 0.f; + return tmp; + } + else { + btScalar s = btSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]); + if (s > SIMD_EPSILON) { + btScalar d = m_radius / s; + btVector3 tmp; + tmp[coneindices[0]] = v[coneindices[0]] * d; + tmp[coneindices[1]] = -halfHeight; + tmp[coneindices[2]] = v[coneindices[2]] * d; + return tmp; + } + else { + btVector3 tmp; + tmp[coneindices[0]] = 0.f; + tmp[coneindices[1]] = -halfHeight; + tmp[coneindices[2]] = 0.f; + return tmp; + } + } + +} + +btVector3 btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const +{ + return coneLocalSupport(vec); +} + +void btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + const btVector3& vec = vectors[i]; + supportVerticesOut[i] = coneLocalSupport(vec); + } +} + + +btVector3 btConeShape::localGetSupportingVertex(const btVector3& vec) const +{ + btVector3 supVertex = coneLocalSupport(vec); + if ( getMargin()!=0.f ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h new file mode 100644 index 00000000000..0fd3ce177fe --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -0,0 +1,83 @@ +/* +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 CONE_MINKOWSKI_H +#define CONE_MINKOWSKI_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +/// implements cone shape interface +class btConeShape : public btConvexShape + +{ + + float m_sinAngle; + float m_radius; + float m_height; + + btVector3 coneLocalSupport(const btVector3& v) const; + + +public: + btConeShape (btScalar radius,btScalar height); + + 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; + + float getRadius() const { return m_radius;} + float getHeight() const { return m_height;} + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) + { + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin,aabbMax; + getAabb(identity,aabbMin,aabbMax); + + btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + float margin = getMargin(); + + btScalar lx=2.f*(halfExtents.x()+margin); + btScalar ly=2.f*(halfExtents.y()+margin); + btScalar lz=2.f*(halfExtents.z()+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + +// inertia.x() = scaledmass * (y2+z2); +// inertia.y() = scaledmass * (x2+z2); +// inertia.z() = scaledmass * (x2+y2); + } + + + + virtual int getShapeType() const { return CONE_SHAPE_PROXYTYPE; } + + virtual char* getName()const + { + return "Cone"; + } +}; + + +#endif //CONE_MINKOWSKI_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp new file mode 100644 index 00000000000..7a4c7ebf5c0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -0,0 +1,166 @@ +/* +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 "btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" + + + +btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int stride) +{ + m_points.resize(numPoints); + unsigned char* pointsBaseAddress = (unsigned char*)points; + + for (int i=0;i<numPoints;i++) + { + btPoint3* point = (btPoint3*)(pointsBaseAddress + i*stride); + m_points[i] = point[0]; + } +} + +btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + btVector3 supVec(0.f,0.f,0.f); + btScalar newDot,maxDot = -1e30f; + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / btSqrt(lenSqr ); + vec *= rlen; + } + + + for (size_t i=0;i<m_points.size();i++) + { + btPoint3 vtx = m_points[i] * m_localScaling; + + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; +} + +void btConvexHullShape::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] = -1e30f; + } + } + for (size_t i=0;i<m_points.size();i++) + { + btPoint3 vtx = m_points[i] * m_localScaling; + + 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 btConvexHullShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=0.f ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + 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 btConvexHullShape with the Raytracer Demo +int btConvexHullShape::getNumVertices() const +{ + return m_points.size(); +} + +int btConvexHullShape::getNumEdges() const +{ + return m_points.size(); +} + +void btConvexHullShape::getEdge(int i,btPoint3& pa,btPoint3& pb) const +{ + + int index0 = i%m_points.size(); + int index1 = (i+1)%m_points.size(); + pa = m_points[index0]*m_localScaling; + pb = m_points[index1]*m_localScaling; +} + +void btConvexHullShape::getVertex(int i,btPoint3& vtx) const +{ + vtx = m_points[i]*m_localScaling; +} + +int btConvexHullShape::getNumPlanes() const +{ + return 0; +} + +void btConvexHullShape::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const +{ + assert(0); +} + +//not yet +bool btConvexHullShape::isInside(const btPoint3& pt,btScalar tolerance) const +{ + assert(0); + return false; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h new file mode 100644 index 00000000000..afe7dd8f7a9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -0,0 +1,67 @@ +/* +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_HULL_SHAPE_H +#define CONVEX_HULL_SHAPE_H + +#include "btPolyhedralConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +#include <vector> + +///ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices) +///No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. +///on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash. +///(memory is much slower then the cpu) +class btConvexHullShape : public btPolyhedralConvexShape +{ + std::vector<btPoint3> m_points; + +public: + ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory. + ///It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. + ///btConvexHullShape make an internal copy of the points. + btConvexHullShape(const float* points=0,int numPoints=0, int stride=sizeof(btPoint3)); + + void addPoint(const btPoint3& point) + { + m_points.push_back(point); + } + 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; + + + virtual int getShapeType()const { return CONVEX_HULL_SHAPE_PROXYTYPE; } + + //debugging + virtual char* getName()const {return "Convex";} + + + virtual int getNumVertices() const; + virtual int getNumEdges() const; + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; + virtual void getVertex(int i,btPoint3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const; + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; + + + +}; + + +#endif //CONVEX_HULL_SHAPE_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp new file mode 100644 index 00000000000..9537235ff8a --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -0,0 +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. +*/ + +#include "btConvexShape.h" + +btConvexShape::btConvexShape() +: m_localScaling(1.f,1.f,1.f), +m_collisionMargin(CONVEX_DISTANCE_MARGIN) +{ +} + + +void btConvexShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} + + + +void btConvexShape::getAabbSlow(const btTransform& trans,btVector3&minAabb,btVector3&maxAabb) const +{ + + btScalar margin = getMargin(); + for (int i=0;i<3;i++) + { + btVector3 vec(0.f,0.f,0.f); + vec[i] = 1.f; + + btVector3 sv = localGetSupportingVertex(vec*trans.getBasis()); + + btVector3 tmp = trans(sv); + maxAabb[i] = tmp[i]+margin; + vec[i] = -1.f; + tmp = trans(localGetSupportingVertex(vec*trans.getBasis())); + minAabb[i] = tmp[i]-margin; + } +}; + +btVector3 btConvexShape::localGetSupportingVertex(const btVector3& vec)const + { + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=0.f ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; + + } + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h new file mode 100644 index 00000000000..3ffde1ba5ed --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -0,0 +1,92 @@ +/* +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_SHAPE_INTERFACE1 +#define CONVEX_SHAPE_INTERFACE1 + +#include "btCollisionShape.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" +#include <vector> +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +//todo: get rid of this btConvexCastResult thing! +struct btConvexCastResult; + + +/// btConvexShape is an abstract shape interface. +/// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. +/// used in combination with GJK or btConvexCast +class btConvexShape : public btCollisionShape +{ + +protected: + + //local scaling. collisionMargin is not scaled ! + btVector3 m_localScaling; + + btVector3 m_implicitShapeDimensions; + + btScalar m_collisionMargin; + +public: + btConvexShape(); + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec) const= 0; + + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; + + const btVector3& getImplicitShapeDimensions() const + { + return m_implicitShapeDimensions; + } + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + getAabbSlow(t,aabbMin,aabbMax); + } + + + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const + { + return m_localScaling; + } + + + virtual void setMargin(float margin) + { + m_collisionMargin = margin; + } + virtual float getMargin() const + { + return m_collisionMargin; + } + + +}; + + + +#endif //CONVEX_SHAPE_INTERFACE1 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp new file mode 100644 index 00000000000..9d8e7e1408e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp @@ -0,0 +1,193 @@ +/* +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) +:m_stridingMesh(meshInterface) +{ +} + + + + +///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(0.f,0.f,0.f), + m_maxDot(-1e30f), + m_supportVecLocal(supportVecLocal) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + 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(0.f,0.f,0.f); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / btSqrt(lenSqr ); + vec *= rlen; + } + + LocalSupportVertexCallback supportCallback(vec); + btVector3 aabbMax(1e30f,1e30f,1e30f); + 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] = -1e30f; + } + } + + //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(1e30f,1e30f,1e30f); + m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + supportVerticesOut[j] = supportCallback.GetSupportVertexLocal(); + } + +} + + + +btVector3 btConvexTriangleMeshShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=0.f ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + 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 i,btPoint3& pa,btPoint3& pb) const +{ + assert(0); +} + +void btConvexTriangleMeshShape::getVertex(int i,btPoint3& vtx) const +{ + assert(0); +} + +int btConvexTriangleMeshShape::getNumPlanes() const +{ + return 0; +} + +void btConvexTriangleMeshShape::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const +{ + assert(0); +} + +//not yet +bool btConvexTriangleMeshShape::isInside(const btPoint3& pt,btScalar tolerance) const +{ + assert(0); + return false; +} + + + +void btConvexTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_stridingMesh->setScaling(scaling); +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h new file mode 100644 index 00000000000..86e871c6c0b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -0,0 +1,51 @@ +#ifndef CONVEX_TRIANGLEMESH_SHAPE_H +#define CONVEX_TRIANGLEMESH_SHAPE_H + + +#include "btPolyhedralConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +#include <vector> + +/// btConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use btConvexHullShape instead. +/// It uses the btStridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data. +class btConvexTriangleMeshShape : public btPolyhedralConvexShape +{ + + class btStridingMeshInterface* m_stridingMesh; + +public: + btConvexTriangleMeshShape(btStridingMeshInterface* meshInterface); + + class btStridingMeshInterface* getStridingMesh() + { + return m_stridingMesh; + } + + 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; + + virtual int getShapeType()const { return CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE; } + + //debugging + virtual char* getName()const {return "ConvexTrimesh";} + + virtual int getNumVertices() const; + virtual int getNumEdges() const; + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; + virtual void getVertex(int i,btPoint3& vtx) const; + virtual int getNumPlanes() const; + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const; + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; + + + void setLocalScaling(const btVector3& scaling); + +}; + + + +#endif //CONVEX_TRIANGLEMESH_SHAPE_H + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp new file mode 100644 index 00000000000..16b263474f0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -0,0 +1,196 @@ +/* +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 "btCylinderShape.h" +#include "LinearMath/btPoint3.h" + +btCylinderShape::btCylinderShape (const btVector3& halfExtents) +:btBoxShape(halfExtents) +{ + +} + + +btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents) +:btCylinderShape(halfExtents) +{ +} + + +btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents) +:btCylinderShape(halfExtents) +{ +} + + + +inline btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v) +{ +const int cylinderUpAxis = 0; +const int XX = 1; +const int YY = 0; +const int ZZ = 2; + + //mapping depends on how cylinder local orientation is + // extents of the cylinder is: X,Y is for radius, and Z for height + + + float radius = halfExtents[XX]; + float halfHeight = halfExtents[cylinderUpAxis]; + + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return tmp; + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return tmp; + } + + +} + + + + + + +inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) +{ + +const int cylinderUpAxis = 1; +const int XX = 0; +const int YY = 1; +const int ZZ = 2; + + + float radius = halfExtents[XX]; + float halfHeight = halfExtents[cylinderUpAxis]; + + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return tmp; + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return tmp; + } + +} + +inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v) +{ +const int cylinderUpAxis = 2; +const int XX = 0; +const int YY = 2; +const int ZZ = 1; + + //mapping depends on how cylinder local orientation is + // extents of the cylinder is: X,Y is for radius, and Z for height + + + float radius = halfExtents[XX]; + float halfHeight = halfExtents[cylinderUpAxis]; + + + btVector3 tmp; + btScalar d ; + + btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != btScalar(0.0)) + { + d = radius / s; + tmp[XX] = v[XX] * d; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = v[ZZ] * d; + return tmp; + } + else + { + tmp[XX] = radius; + tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight; + tmp[ZZ] = btScalar(0.0); + return tmp; + } + + +} + +btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return CylinderLocalSupportX(getHalfExtents(),vec); +} + + +btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return CylinderLocalSupportZ(getHalfExtents(),vec); +} +btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return CylinderLocalSupportY(getHalfExtents(),vec); +} + +void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtents(),vectors[i]); + } +} + +void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtents(),vectors[i]); + } +} + + + + +void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtents(),vectors[i]); + } +} + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h new file mode 100644 index 00000000000..bb0e4298abc --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -0,0 +1,140 @@ +/* +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 CYLINDER_MINKOWSKI_H +#define CYLINDER_MINKOWSKI_H + +#include "btBoxShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types +#include "LinearMath/btVector3.h" + +/// implements cylinder shape interface +class btCylinderShape : public btBoxShape + +{ + +public: + btCylinderShape (const btVector3& halfExtents); + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const + { + getAabbSlow(t,aabbMin,aabbMax); + } + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const + { + + btVector3 supVertex; + supVertex = localGetSupportingVertexWithoutMargin(vec); + + if ( getMargin()!=0.f ) + { + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + } + return supVertex; + } + + + //use box inertia + // virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + virtual int getShapeType() const + { + return CYLINDER_SHAPE_PROXYTYPE; + } + + virtual int getUpAxis() const + { + return 1; + } + + virtual float getRadius() const + { + return getHalfExtents().getX(); + } + + //debugging + virtual char* getName()const + { + return "CylinderY"; + } + + + +}; + +class btCylinderShapeX : public btCylinderShape +{ +public: + btCylinderShapeX (const btVector3& halfExtents); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual int getUpAxis() const + { + return 0; + } + //debugging + virtual char* getName()const + { + return "CylinderX"; + } + + virtual float getRadius() const + { + return getHalfExtents().getY(); + } + +}; + +class btCylinderShapeZ : public btCylinderShape +{ +public: + btCylinderShapeZ (const btVector3& halfExtents); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual int getUpAxis() const + { + return 2; + } + //debugging + virtual char* getName()const + { + return "CylinderZ"; + } + + virtual float getRadius() const + { + return getHalfExtents().getX(); + } + +}; + + +#endif //CYLINDER_MINKOWSKI_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp new file mode 100644 index 00000000000..c63f4a9c571 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.cpp @@ -0,0 +1,49 @@ +/* +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 "btEmptyShape.h" + + +#include "btCollisionShape.h" + + +btEmptyShape::btEmptyShape() +{ +} + + +btEmptyShape::~btEmptyShape() +{ +} + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btEmptyShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 margin(getMargin(),getMargin(),getMargin()); + + aabbMin = t.getOrigin() - margin; + + aabbMax = t.getOrigin() + margin; + +} + +void btEmptyShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + assert(0); +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h new file mode 100644 index 00000000000..c92f42cdee8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -0,0 +1,71 @@ +/* +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 EMPTY_SHAPE_H +#define EMPTY_SHAPE_H + +#include "btConcaveShape.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "LinearMath/btMatrix3x3.h" +#include <vector> +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + + + + +/// btEmptyShape is a collision shape without actual collision detection. +///It can be replaced by another shape during runtime +class btEmptyShape : public ConcaveShape +{ +public: + btEmptyShape(); + + virtual ~btEmptyShape(); + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + + virtual void setLocalScaling(const btVector3& scaling) + { + m_localScaling = scaling; + } + virtual const btVector3& getLocalScaling() const + { + return m_localScaling; + } + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + virtual int getShapeType() const { return EMPTY_SHAPE_PROXYTYPE;} + + + virtual char* getName()const + { + return "Empty"; + } + + +protected: + btVector3 m_localScaling; + +}; + + + +#endif //EMPTY_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp new file mode 100644 index 00000000000..4bf8c478a53 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -0,0 +1,56 @@ +/* +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 "btMinkowskiSumShape.h" + + +btMinkowskiSumShape::btMinkowskiSumShape(btConvexShape* shapeA,btConvexShape* shapeB) +:m_shapeA(shapeA), +m_shapeB(shapeB) +{ + m_transA.setIdentity(); + m_transB.setIdentity(); +} + +btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); + return supVertexA + supVertexB; +} + +void btMinkowskiSumShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + //todo: could make recursive use of batching. probably this shape is not used frequently. + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i] = localGetSupportingVertexWithoutMargin(vectors[i]); + } + +} + + + +float btMinkowskiSumShape::getMargin() const +{ + return m_shapeA->getMargin() + m_shapeB->getMargin(); +} + + +void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + assert(0); + inertia.setValue(0,0,0); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h new file mode 100644 index 00000000000..e974f57a4c2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -0,0 +1,62 @@ +/* +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 MINKOWSKI_SUM_SHAPE_H +#define MINKOWSKI_SUM_SHAPE_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +/// btMinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. +class btMinkowskiSumShape : public btConvexShape +{ + + btTransform m_transA; + btTransform m_transB; + btConvexShape* m_shapeA; + btConvexShape* m_shapeB; + +public: + + btMinkowskiSumShape(btConvexShape* shapeA,btConvexShape* shapeB); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + void setTransformA(const btTransform& transA) { m_transA = transA;} + void setTransformB(const btTransform& transB) { m_transB = transB;} + + const btTransform& getTransformA()const { return m_transA;} + const btTransform& GetTransformB()const { return m_transB;} + + + virtual int getShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; } + + virtual float getMargin() const; + + const btConvexShape* getShapeA() const { return m_shapeA;} + const btConvexShape* getShapeB() const { return m_shapeB;} + + virtual char* getName()const + { + return "MinkowskiSum"; + } +}; + +#endif //MINKOWSKI_SUM_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp new file mode 100644 index 00000000000..aaadb82eb4b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp @@ -0,0 +1,148 @@ +/* +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 "btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" +#include "LinearMath/btQuaternion.h" + +btMultiSphereShape::btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres) +:m_inertiaHalfExtents(inertiaHalfExtents) +{ + m_minRadius = 1e30f; + + m_numSpheres = numSpheres; + for (int i=0;i<m_numSpheres;i++) + { + m_localPositions[i] = positions[i]; + m_radi[i] = radi[i]; + if (radi[i] < m_minRadius) + m_minRadius = radi[i]; + } + setMargin(m_minRadius); + +} + + + + + btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + int i; + btVector3 supVec(0,0,0); + + btScalar maxDot(-1e30f); + + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / btSqrt(lenSqr ); + vec *= rlen; + } + + btVector3 vtx; + btScalar newDot; + + const btVector3* pos = &m_localPositions[0]; + const btScalar* rad = &m_radi[0]; + + for (i=0;i<m_numSpheres;i++) + { + vtx = (*pos) +vec*((*rad)-m_minRadius); + pos++; + rad++; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + + void btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + + for (int j=0;j<numVectors;j++) + { + btScalar maxDot(-1e30f); + + const btVector3& vec = vectors[j]; + + btVector3 vtx; + btScalar newDot; + + const btVector3* pos = &m_localPositions[0]; + const btScalar* rad = &m_radi[0]; + + for (int i=0;i<m_numSpheres;i++) + { + vtx = (*pos) +vec*((*rad)-m_minRadius); + pos++; + rad++; + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + } +} + + + + + + + + +void btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //as an approximation, take the inertia of the box that bounds the spheres + + btTransform ident; + ident.setIdentity(); +// btVector3 aabbMin,aabbMax; + +// getAabb(ident,aabbMin,aabbMax); + + btVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* 0.5f; + + float margin = CONVEX_DISTANCE_MARGIN; + + btScalar lx=2.f*(halfExtents[0]+margin); + btScalar ly=2.f*(halfExtents[1]+margin); + btScalar lz=2.f*(halfExtents[2]+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * 0.08333333f; + + inertia[0] = scaledmass * (y2+z2); + inertia[1] = scaledmass * (x2+z2); + inertia[2] = scaledmass * (x2+y2); + +} + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h new file mode 100644 index 00000000000..6a9151df281 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -0,0 +1,62 @@ +/* +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 MULTI_SPHERE_MINKOWSKI_H +#define MULTI_SPHERE_MINKOWSKI_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +#define MAX_NUM_SPHERES 5 + +///btMultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) +class btMultiSphereShape : public btConvexShape + +{ + + btVector3 m_localPositions[MAX_NUM_SPHERES]; + btScalar m_radi[MAX_NUM_SPHERES]; + btVector3 m_inertiaHalfExtents; + + int m_numSpheres; + float m_minRadius; + + + + + +public: + btMultiSphereShape (const btVector3& inertiaHalfExtents,const btVector3* positions,const btScalar* radi,int numSpheres); + + ///CollisionShape Interface + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + /// btConvexShape Interface + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + + virtual int getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } + + virtual char* getName()const + { + return "MultiSphere"; + } + +}; + + +#endif //MULTI_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp new file mode 100644 index 00000000000..18b796b39b5 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -0,0 +1,274 @@ +/* +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 "btOptimizedBvh.h" +#include "btStridingMeshInterface.h" +#include "LinearMath/btAabbUtil2.h" + + + +void btOptimizedBvh::build(btStridingMeshInterface* triangles) +{ + //int countTriangles = 0; + + + + // NodeArray triangleNodes; + + struct NodeTriangleCallback : public btInternalTriangleIndexCallback + { + NodeArray& m_triangleNodes; + + NodeTriangleCallback(NodeArray& triangleNodes) + :m_triangleNodes(triangleNodes) + { + + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + + btOptimizedBvhNode node; + node.m_aabbMin = btVector3(1e30f,1e30f,1e30f); + node.m_aabbMax = btVector3(-1e30f,-1e30f,-1e30f); + node.m_aabbMin.setMin(triangle[0]); + node.m_aabbMax.setMax(triangle[0]); + node.m_aabbMin.setMin(triangle[1]); + node.m_aabbMax.setMax(triangle[1]); + node.m_aabbMin.setMin(triangle[2]); + node.m_aabbMax.setMax(triangle[2]); + + node.m_escapeIndex = -1; + node.m_leftChild = 0; + node.m_rightChild = 0; + + + //for child nodes + node.m_subPart = partId; + node.m_triangleIndex = triangleIndex; + + + m_triangleNodes.push_back(node); + } + }; + + + + NodeTriangleCallback callback(m_leafNodes); + + btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + btVector3 aabbMax(1e30f,1e30f,1e30f); + + triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + + //now we have an array of leafnodes in m_leafNodes + + m_contiguousNodes = new btOptimizedBvhNode[2*m_leafNodes.size()]; + m_curNodeIndex = 0; + + m_rootNode1 = buildTree(m_leafNodes,0,m_leafNodes.size()); + + + ///create the leafnodes first +// btOptimizedBvhNode* leafNodes = new btOptimizedBvhNode; +} + +btOptimizedBvh::~btOptimizedBvh() +{ + if (m_contiguousNodes) + delete []m_contiguousNodes; +} + +btOptimizedBvhNode* btOptimizedBvh::buildTree (NodeArray& leafNodes,int startIndex,int endIndex) +{ + btOptimizedBvhNode* internalNode; + + int splitAxis, splitIndex, i; + int numIndices =endIndex-startIndex; + int curIndex = m_curNodeIndex; + + assert(numIndices>0); + + if (numIndices==1) + { + return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]); + } + //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + splitAxis = calcSplittingAxis(leafNodes,startIndex,endIndex); + + splitIndex = sortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis); + + internalNode = &m_contiguousNodes[m_curNodeIndex++]; + + internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f); + internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f); + + for (i=startIndex;i<endIndex;i++) + { + internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax); + internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin); + } + + + + //internalNode->m_escapeIndex; + internalNode->m_leftChild = buildTree(leafNodes,startIndex,splitIndex); + internalNode->m_rightChild = buildTree(leafNodes,splitIndex,endIndex); + + internalNode->m_escapeIndex = m_curNodeIndex - curIndex; + return internalNode; +} + +int btOptimizedBvh::sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + float splitValue; + + btVector3 means(0.f,0.f,0.f); + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + means+=center; + } + means *= (1.f/(float)numIndices); + + splitValue = means[splitAxis]; + + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + if (center[splitAxis] > splitValue) + { + //swap + btOptimizedBvhNode tmp = leafNodes[i]; + leafNodes[i] = leafNodes[splitIndex]; + leafNodes[splitIndex] = tmp; + splitIndex++; + } + } + if ((splitIndex==startIndex) || (splitIndex == (endIndex-1))) + { + splitIndex = startIndex+ (numIndices>>1); + } + return splitIndex; +} + + +int btOptimizedBvh::calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex) +{ + int i; + + btVector3 means(0.f,0.f,0.f); + btVector3 variance(0.f,0.f,0.f); + int numIndices = endIndex-startIndex; + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + means+=center; + } + means *= (1.f/(float)numIndices); + + for (i=startIndex;i<endIndex;i++) + { + btVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin); + btVector3 diff2 = center-means; + diff2 = diff2 * diff2; + variance += diff2; + } + variance *= (1.f/ ((float)numIndices-1) ); + + return variance.maxAxis(); +} + + + +void btOptimizedBvh::reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //either choose recursive traversal (walkTree) or stackless (walkStacklessTree) + + //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + + walkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); +} + +void btOptimizedBvh::walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + bool isLeafNode, aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); + if (aabbOverlap) + { + isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + if (isLeafNode) + { + nodeCallback->processNode(rootNode); + } else + { + walkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax); + walkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax); + } + } + +} + +int maxIterations = 0; + +void btOptimizedBvh::walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + int escapeIndex, curIndex = 0; + int walkIterations = 0; + bool aabbOverlap, isLeafNode; + + while (curIndex < m_curNodeIndex) + { + //catch bugs in tree data + assert (walkIterations < m_curNodeIndex); + + walkIterations++; + aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax); + isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild); + + if (isLeafNode && aabbOverlap) + { + nodeCallback->processNode(rootNode); + } + + if (aabbOverlap || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->m_escapeIndex; + rootNode += escapeIndex; + curIndex += escapeIndex; + } + + } + + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} + + +void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h new file mode 100644 index 00000000000..96172c4e298 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -0,0 +1,100 @@ +/* +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 OPTIMIZED_BVH_H +#define OPTIMIZED_BVH_H +#include "LinearMath/btVector3.h" +#include <vector> + +class btStridingMeshInterface; + +/// btOptimizedBvhNode contains both internal and leaf node information. +/// It hasn't been optimized yet for storage. Some obvious optimizations are: +/// Removal of the pointers (can already be done, they are not used for traversal) +/// and storing aabbmin/max as quantized integers. +/// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle +/// meshes stored in a non-uniform way (like batches/subparts of triangle-fans +struct btOptimizedBvhNode +{ + + btVector3 m_aabbMin; + btVector3 m_aabbMax; + +//these 2 pointers are obsolete, the stackless traversal just uses the escape index + btOptimizedBvhNode* m_leftChild; + btOptimizedBvhNode* m_rightChild; + + int m_escapeIndex; + + //for child nodes + int m_subPart; + int m_triangleIndex; + +}; + +class btNodeOverlapCallback +{ +public: + virtual ~btNodeOverlapCallback() {}; + + virtual void processNode(const btOptimizedBvhNode* node) = 0; +}; + +typedef std::vector<btOptimizedBvhNode> NodeArray; + + +///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) +class btOptimizedBvh +{ + btOptimizedBvhNode* m_rootNode1; + + btOptimizedBvhNode* m_contiguousNodes; + int m_curNodeIndex; + + int m_numNodes; + + NodeArray m_leafNodes; + +public: + btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) { } + virtual ~btOptimizedBvh(); + + void build(btStridingMeshInterface* triangles); + + btOptimizedBvhNode* buildTree (NodeArray& leafNodes,int startIndex,int endIndex); + + int calcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex); + + int sortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis); + + void walkTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void walkStacklessTree(btOptimizedBvhNode* rootNode,btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + + //OptimizedBvhNode* GetRootNode() { return m_rootNode1;} + + int getNumNodes() { return m_numNodes;} + + void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + +}; + + +#endif //OPTIMIZED_BVH_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp new file mode 100644 index 00000000000..6f2272cc454 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -0,0 +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. +*/ + +#include <BulletCollision/CollisionShapes/btPolyhedralConvexShape.h> + +btPolyhedralConvexShape::btPolyhedralConvexShape() +:m_optionalHull(0) +{ + +} + + + +btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const +{ + int i; + btVector3 supVec(0,0,0); + + btScalar maxDot(-1e30f); + + btVector3 vec = vec0; + btScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / btSqrt(lenSqr ); + vec *= rlen; + } + + btVector3 vtx; + btScalar newDot; + + for (i=0;i<getNumVertices();i++) + { + getVertex(i,vtx); + newDot = vec.dot(vtx); + if (newDot > maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + +void btPolyhedralConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + int i; + + btVector3 vtx; + btScalar newDot; + + for (i=0;i<numVectors;i++) + { + supportVerticesOut[i][3] = -1e30f; + } + + for (int j=0;j<numVectors;j++) + { + + const btVector3& vec = vectors[j]; + + for (i=0;i<getNumVertices();i++) + { + getVertex(i,vtx); + 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; + } + } + } +} + + + +void btPolyhedralConvexShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //not yet, return box inertia + + float margin = getMargin(); + + btTransform ident; + ident.setIdentity(); + btVector3 aabbMin,aabbMax; + getAabb(ident,aabbMin,aabbMax); + btVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + btScalar lx=2.f*(halfExtents.x()+margin); + btScalar ly=2.f*(halfExtents.y()+margin); + btScalar lz=2.f*(halfExtents.z()+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h new file mode 100644 index 00000000000..a404504ba86 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -0,0 +1,55 @@ +/* +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 BU_SHAPE +#define BU_SHAPE + +#include <LinearMath/btPoint3.h> +#include <LinearMath/btMatrix3x3.h> +#include <BulletCollision/CollisionShapes/btConvexShape.h> + + +///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. +class btPolyhedralConvexShape : public btConvexShape +{ + +public: + + btPolyhedralConvexShape(); + + //brute force implementations + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + + + virtual int getNumVertices() const = 0 ; + virtual int getNumEdges() const = 0; + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const = 0; + virtual void getVertex(int i,btPoint3& vtx) const = 0; + virtual int getNumPlanes() const = 0; + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i ) const = 0; +// virtual int getIndex(int i) const = 0 ; + + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const = 0; + + /// optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp + class Hull* m_optionalHull; + +}; + +#endif //BU_SHAPE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp new file mode 100644 index 00000000000..39e458c0f22 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.cpp @@ -0,0 +1,74 @@ +/* +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 "btSphereShape.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "LinearMath/btQuaternion.h" + + +btSphereShape ::btSphereShape (btScalar radius) +{ + m_implicitShapeDimensions.setX(radius); +} + +btVector3 btSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return btVector3(0.f,0.f,0.f); +} + +void btSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i<numVectors;i++) + { + supportVerticesOut[i].setValue(0.f,0.f,0.f); + } +} + + +btVector3 btSphereShape::localGetSupportingVertex(const btVector3& vec)const +{ + btVector3 supVertex; + supVertex = localGetSupportingVertexWithoutMargin(vec); + + btVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= getMargin() * vecnorm; + return supVertex; +} + + +//broken due to scaling +void btSphereShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + const btVector3& center = t.getOrigin(); + btVector3 extent(getMargin(),getMargin(),getMargin()); + aabbMin = center - extent; + aabbMax = center + extent; +} + + + +void btSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + btScalar elem = 0.4f * mass * getMargin()*getMargin(); + inertia[0] = inertia[1] = inertia[2] = elem; + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h new file mode 100644 index 00000000000..2db6a872b40 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -0,0 +1,63 @@ +/* +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_MINKOWSKI_H +#define SPHERE_MINKOWSKI_H + +#include "btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +///btSphereShape implements an implicit (getSupportingVertex) Sphere +class btSphereShape : public btConvexShape + +{ + +public: + btSphereShape (btScalar radius); + + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + //notice that the vectors should be unit length + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia); + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual int getShapeType() const { return SPHERE_SHAPE_PROXYTYPE; } + + btScalar getRadius() const { return m_implicitShapeDimensions.getX();} + + //debugging + virtual char* getName()const {return "SPHERE";} + + virtual void setMargin(float margin) + { + btConvexShape::setMargin(margin); + } + virtual float getMargin() const + { + //to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case + //this means, non-uniform scaling is not supported anymore + return m_localScaling.getX() * getRadius() + btConvexShape::getMargin(); + } + + +}; + + +#endif //SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp new file mode 100644 index 00000000000..8c7022ba83b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -0,0 +1,100 @@ +/* +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 "btStaticPlaneShape.h" + +#include "LinearMath/btTransformUtil.h" + + +btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant) +:m_planeNormal(planeNormal), +m_planeConstant(planeConstant), +m_localScaling(0.f,0.f,0.f) +{ +} + + +btStaticPlaneShape::~btStaticPlaneShape() +{ +} + + + +void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btVector3 infvec (1e30f,1e30f,1e30f); + + btVector3 center = m_planeNormal*m_planeConstant; + aabbMin = center + infvec*m_planeNormal; + aabbMax = aabbMin; + aabbMin.setMin(center - infvec*m_planeNormal); + aabbMax.setMax(center - infvec*m_planeNormal); + + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); + +} + + + + +void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + + btVector3 halfExtents = (aabbMax - aabbMin) * 0.5f; + btScalar radius = halfExtents.length(); + btVector3 center = (aabbMax + aabbMin) * 0.5f; + + //this is where the triangles are generated, given AABB and plane equation (normal/constant) + + btVector3 tangentDir0,tangentDir1; + + //tangentDir0/tangentDir1 can be precalculated + btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); + + btVector3 supVertex0,supVertex1; + + btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; + + btVector3 triangle[3]; + triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius; + triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius; + + callback->processTriangle(triangle,0,0); + + triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius; + triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius; + triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius; + + callback->processTriangle(triangle,0,1); + +} + +void btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //moving concave objects not supported + + inertia.setValue(0.f,0.f,0.f); +} + +void btStaticPlaneShape::setLocalScaling(const btVector3& scaling) +{ + m_localScaling = scaling; +} +const btVector3& btStaticPlaneShape::getLocalScaling() const +{ + return m_localScaling; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h new file mode 100644 index 00000000000..7414d470d7d --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -0,0 +1,61 @@ +/* +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 STATIC_PLANE_SHAPE_H +#define STATIC_PLANE_SHAPE_H + +#include "BulletCollision/CollisionShapes/btConcaveShape.h" + + +///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB. +///Assumed is that the other objects is not also infinite, so a reasonable sized AABB. +class btStaticPlaneShape : public ConcaveShape +{ +protected: + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + btVector3 m_planeNormal; + btScalar m_planeConstant; + btVector3 m_localScaling; + +public: + btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant); + + virtual ~btStaticPlaneShape(); + + + virtual int getShapeType() const + { + return STATIC_PLANE_PROXYTYPE; + } + + 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); + + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + + + //debugging + virtual char* getName()const {return "STATICPLANE";} + + +}; + +#endif //STATIC_PLANE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp new file mode 100644 index 00000000000..f7507b29394 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -0,0 +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 "btStridingMeshInterface.h" + +btStridingMeshInterface::~btStridingMeshInterface() +{ + +} + + +void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + int numtotalphysicsverts = 0; + int part,graphicssubparts = getNumSubParts(); + const unsigned char * vertexbase; + const unsigned char * indexbase; + int indexstride; + PHY_ScalarType type; + PHY_ScalarType gfxindextype; + int stride,numverts,numtriangles; + int gfxindex; + btVector3 triangle[3]; + float* graphicsbase; + + btVector3 meshScaling = getScaling(); + + ///if the number of parts is big, the performance might drop due to the innerloop switch on indextype + for (part=0;part<graphicssubparts ;part++) + { + getLockedReadOnlyVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part); + numtotalphysicsverts+=numtriangles*3; //upper bound + + switch (gfxindextype) + { + case PHY_INTEGER: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + int* tri_indices= (int*)(indexbase+gfxindex*indexstride); + graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + case PHY_SHORT: + { + for (gfxindex=0;gfxindex<numtriangles;gfxindex++) + { + short int* tri_indices= (short int*)(indexbase+gfxindex*indexstride); + graphicsbase = (float*)(vertexbase+tri_indices[0]*stride); + triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[1]*stride); + triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + graphicsbase = (float*)(vertexbase+tri_indices[2]*stride); + triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(), graphicsbase[2]*meshScaling.getZ()); + callback->internalProcessTriangleIndex(triangle,part,gfxindex); + } + break; + } + default: + ASSERT((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT)); + } + + unLockReadOnlyVertexBase(part); + } +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h new file mode 100644 index 00000000000..830cbb28200 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -0,0 +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. +*/ + +#ifndef STRIDING_MESHINTERFACE_H +#define STRIDING_MESHINTERFACE_H + +#include "LinearMath/btVector3.h" +#include "btTriangleCallback.h" + +/// PHY_ScalarType enumerates possible scalar types. +/// See the btStridingMeshInterface for its use +typedef enum PHY_ScalarType { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88 +} PHY_ScalarType; + +/// btStridingMeshInterface is the interface class for high performance access to triangle meshes +/// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. +class btStridingMeshInterface +{ + protected: + + btVector3 m_scaling; + + public: + btStridingMeshInterface() :m_scaling(1.f,1.f,1.f) + { + + } + + virtual ~btStridingMeshInterface(); + + + + void InternalProcessAllTriangles(btInternalTriangleIndexCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + + /// get read and write access to a subpart of a triangle mesh + /// this subpart has a continuous array of vertices and indices + /// in this way the mesh can be handled as chunks of memory with striding + /// very similar to OpenGL vertexarray support + /// make a call to unLockVertexBase when the read and write access is finished + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0)=0; + + virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const=0; + + /// unLockVertexBase finishes the access to a subpart of the triangle mesh + /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + virtual void unLockVertexBase(int subpart)=0; + + virtual void unLockReadOnlyVertexBase(int subpart) const=0; + + + /// getNumSubParts returns the number of seperate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const=0; + + virtual void preallocateVertices(int numverts)=0; + virtual void preallocateIndices(int numindices)=0; + + const btVector3& getScaling() const { + return m_scaling; + } + void setScaling(const btVector3& scaling) + { + m_scaling = scaling; + } + + +}; + +#endif //STRIDING_MESHINTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp new file mode 100644 index 00000000000..7cb40c4fac1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.cpp @@ -0,0 +1,193 @@ + +/* +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 "btTetrahedronShape.h" +#include "LinearMath/btMatrix3x3.h" + +btBU_Simplex1to4::btBU_Simplex1to4() +:m_numVertices(0) +{ +} + +btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0) +:m_numVertices(0) +{ + addVertex(pt0); +} + +btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1) +:m_numVertices(0) +{ + addVertex(pt0); + addVertex(pt1); +} + +btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2) +:m_numVertices(0) +{ + addVertex(pt0); + addVertex(pt1); + addVertex(pt2); +} + +btBU_Simplex1to4::btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3) +:m_numVertices(0) +{ + addVertex(pt0); + addVertex(pt1); + addVertex(pt2); + addVertex(pt3); +} + + + + + +void btBU_Simplex1to4::addVertex(const btPoint3& pt) +{ + m_vertices[m_numVertices++] = pt; +} + + +int btBU_Simplex1to4::getNumVertices() const +{ + return m_numVertices; +} + +int btBU_Simplex1to4::getNumEdges() const +{ + //euler formula, F-E+V = 2, so E = F+V-2 + + switch (m_numVertices) + { + case 0: + return 0; + case 1: return 0; + case 2: return 1; + case 3: return 3; + case 4: return 6; + + + } + + return 0; +} + +void btBU_Simplex1to4::getEdge(int i,btPoint3& pa,btPoint3& pb) const +{ + + switch (m_numVertices) + { + + case 2: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 3: + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + + } + break; + case 4: + switch (i) + { + case 0: + pa = m_vertices[0]; + pb = m_vertices[1]; + break; + case 1: + pa = m_vertices[1]; + pb = m_vertices[2]; + break; + case 2: + pa = m_vertices[2]; + pb = m_vertices[0]; + break; + case 3: + pa = m_vertices[0]; + pb = m_vertices[3]; + break; + case 4: + pa = m_vertices[1]; + pb = m_vertices[3]; + break; + case 5: + pa = m_vertices[2]; + pb = m_vertices[3]; + break; + } + + } + + + + +} + +void btBU_Simplex1to4::getVertex(int i,btPoint3& vtx) const +{ + vtx = m_vertices[i]; +} + +int btBU_Simplex1to4::getNumPlanes() const +{ + switch (m_numVertices) + { + case 0: + return 0; + case 1: + return 0; + case 2: + return 0; + case 3: + return 2; + case 4: + return 4; + default: + { + } + } + return 0; +} + + +void btBU_Simplex1to4::getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const +{ + +} + +int btBU_Simplex1to4::getIndex(int i) const +{ + return 0; +} + +bool btBU_Simplex1to4::isInside(const btPoint3& pt,btScalar tolerance) const +{ + return false; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h new file mode 100644 index 00000000000..9e17a248f84 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -0,0 +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 BU_SIMPLEX_1TO4_SHAPE +#define BU_SIMPLEX_1TO4_SHAPE + + +#include <BulletCollision/CollisionShapes/btPolyhedralConvexShape.h> +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" + + +///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). +class btBU_Simplex1to4 : public btPolyhedralConvexShape +{ +protected: + + int m_numVertices; + btPoint3 m_vertices[4]; + +public: + btBU_Simplex1to4(); + + btBU_Simplex1to4(const btPoint3& pt0); + btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1); + btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2); + btBU_Simplex1to4(const btPoint3& pt0,const btPoint3& pt1,const btPoint3& pt2,const btPoint3& pt3); + + + void reset() + { + m_numVertices = 0; + } + + + virtual int getShapeType() const{ return TETRAHEDRAL_SHAPE_PROXYTYPE; } + + void addVertex(const btPoint3& pt); + + //PolyhedralConvexShape interface + + virtual int getNumVertices() const; + + virtual int getNumEdges() const; + + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const; + + virtual void getVertex(int i,btPoint3& vtx) const; + + virtual int getNumPlanes() const; + + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const; + + virtual int getIndex(int i) const; + + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const; + + + ///getName is for debugging + virtual char* getName()const { return "btBU_Simplex1to4";} + +}; + +#endif //BU_SIMPLEX_1TO4_SHAPE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp new file mode 100644 index 00000000000..a020746db5f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.cpp @@ -0,0 +1,28 @@ +/* +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 "btTriangleCallback.h" + +btTriangleCallback::~btTriangleCallback() +{ + +} + + +btInternalTriangleIndexCallback::~btInternalTriangleIndexCallback() +{ + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h new file mode 100644 index 00000000000..7b2337498ec --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -0,0 +1,40 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef TRIANGLE_CALLBACK_H +#define TRIANGLE_CALLBACK_H + +#include "LinearMath/btVector3.h" + + +class btTriangleCallback +{ +public: + + virtual ~btTriangleCallback(); + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) = 0; +}; + +class btInternalTriangleIndexCallback +{ +public: + + virtual ~btInternalTriangleIndexCallback(); + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) = 0; +}; + + + +#endif //TRIANGLE_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp new file mode 100644 index 00000000000..154b7145e68 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp @@ -0,0 +1,65 @@ +/* +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 "btTriangleIndexVertexArray.h" + +btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) +{ + btIndexedMesh mesh; + + mesh.m_numTriangles = numTriangles; + mesh.m_triangleIndexBase = triangleIndexBase; + mesh.m_triangleIndexStride = triangleIndexStride; + mesh.m_numVertices = numVertices; + mesh.m_vertexBase = vertexBase; + mesh.m_vertexStride = vertexStride; + + addIndexedMesh(mesh); + +} + +void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +{ + ASSERT(subpart< getNumSubParts() ); + + btIndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (unsigned char *) mesh.m_vertexBase; + type = PHY_FLOAT; + vertexStride = mesh.m_vertexStride; + + numfaces = mesh.m_numTriangles; + + (*indexbase) = (unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; + indicestype = PHY_INTEGER; +} + +void btTriangleIndexVertexArray::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const +{ + const btIndexedMesh& mesh = m_indexedMeshes[subpart]; + + numverts = mesh.m_numVertices; + (*vertexbase) = (const unsigned char *)mesh.m_vertexBase; + type = PHY_FLOAT; + vertexStride = mesh.m_vertexStride; + + numfaces = mesh.m_numTriangles; + (*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase; + indexstride = mesh.m_triangleIndexStride; + indicestype = PHY_INTEGER; +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h new file mode 100644 index 00000000000..638c8b87fb1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -0,0 +1,82 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_TRIANGLE_INDEX_VERTEX_ARRAY_H +#define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H + +#include "btStridingMeshInterface.h" +#include <vector> + +///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements +///instead of the number of indices, we pass the number of triangles +///todo: explain with pictures +struct btIndexedMesh + { + int m_numTriangles; + int* m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + float* m_vertexBase; + int m_vertexStride; + }; + +///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays. +///Additional meshes can be added using addIndexedMesh +///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. +class btTriangleIndexVertexArray : public btStridingMeshInterface +{ + std::vector<btIndexedMesh> m_indexedMeshes; + + +public: + + + + btTriangleIndexVertexArray() + { + } + + //just to be backwards compatible + btTriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride); + + void addIndexedMesh(const btIndexedMesh& mesh) + { + m_indexedMeshes.push_back(mesh); + } + + + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0); + + virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const; + + /// unLockVertexBase finishes the access to a subpart of the triangle mesh + /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + virtual void unLockVertexBase(int subpart) {} + + virtual void unLockReadOnlyVertexBase(int subpart) const {} + + /// getNumSubParts returns the number of seperate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const { + return (int)m_indexedMeshes.size(); + } + + virtual void preallocateVertices(int numverts){} + virtual void preallocateIndices(int numindices){} + +}; + +#endif //BT_TRIANGLE_INDEX_VERTEX_ARRAY_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp new file mode 100644 index 00000000000..489fe1bbcaa --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.cpp @@ -0,0 +1,61 @@ +/* +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 "btTriangleMesh.h" +#include <assert.h> + +static int myindices[3] = {0,1,2}; + +btTriangleMesh::btTriangleMesh () +{ + +} + +void btTriangleMesh::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +{ + numverts = 3; + *vertexbase = (unsigned char*)&m_triangles[subpart]; + type = PHY_FLOAT; + stride = sizeof(btVector3); + + + numfaces = 1; + *indexbase = (unsigned char*) &myindices[0]; + indicestype = PHY_INTEGER; + indexstride = sizeof(int); + +} + +void btTriangleMesh::getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) const +{ + numverts = 3; + *vertexbase = (unsigned char*)&m_triangles[subpart]; + type = PHY_FLOAT; + stride = sizeof(btVector3); + + + numfaces = 1; + *indexbase = (unsigned char*) &myindices[0]; + indicestype = PHY_INTEGER; + indexstride = sizeof(int); + +} + + + +int btTriangleMesh::getNumSubParts() const +{ + return m_triangles.size(); +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h new file mode 100644 index 00000000000..690d1e849de --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -0,0 +1,73 @@ +/* +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 TRIANGLE_MESH_H +#define TRIANGLE_MESH_H + +#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" +#include <vector> +#include <LinearMath/btVector3.h> + +struct btMyTriangle +{ + btVector3 m_vert0; + btVector3 m_vert1; + btVector3 m_vert2; +}; + +///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the btTriangleMeshShape. +class btTriangleMesh : public btStridingMeshInterface +{ + std::vector<btMyTriangle> m_triangles; + + + public: + btTriangleMesh (); + + void addTriangle(const btVector3& vertex0,const btVector3& vertex1,const btVector3& vertex2) + { + btMyTriangle tri; + tri.m_vert0 = vertex0; + tri.m_vert1 = vertex1; + tri.m_vert2 = vertex2; + m_triangles.push_back(tri); + } + + +//StridingMeshInterface interface implementation + + virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0); + + virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& stride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const; + + /// unLockVertexBase finishes the access to a subpart of the triangle mesh + /// make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished + virtual void unLockVertexBase(int subpart) {} + + virtual void unLockReadOnlyVertexBase(int subpart) const {} + + /// getNumSubParts returns the number of seperate subparts + /// each subpart has a continuous array of vertices and indices + virtual int getNumSubParts() const; + + virtual void preallocateVertices(int numverts){} + virtual void preallocateIndices(int numindices){} + + +}; + +#endif //TRIANGLE_MESH_H + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp new file mode 100644 index 00000000000..cd2bf7261d1 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp @@ -0,0 +1,201 @@ +/* +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 "btTriangleMeshShape.h" +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" +#include "btStridingMeshInterface.h" +#include "LinearMath/btAabbUtil2.h" +#include "BulletCollision/CollisionShapes/btCollisionMargin.h" + +#include "stdio.h" + +btTriangleMeshShape::btTriangleMeshShape(btStridingMeshInterface* meshInterface) +: m_meshInterface(meshInterface) +{ + recalcLocalAabb(); +} + + +btTriangleMeshShape::~btTriangleMeshShape() +{ + +} + + + + +void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const +{ + + btVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); + btVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + + btMatrix3x3 abs_b = trans.getBasis().absolute(); + + btPoint3 center = trans(localCenter); + + btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += btVector3(getMargin(),getMargin(),getMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; + + +} + +void btTriangleMeshShape::recalcLocalAabb() +{ + for (int i=0;i<3;i++) + { + btVector3 vec(0.f,0.f,0.f); + vec[i] = 1.f; + btVector3 tmp = localGetSupportingVertex(vec); + m_localAabbMax[i] = tmp[i]+m_collisionMargin; + vec[i] = -1.f; + tmp = localGetSupportingVertex(vec); + m_localAabbMin[i] = tmp[i]-m_collisionMargin; + } +} + + + +class SupportVertexCallback : public btTriangleCallback +{ + + btVector3 m_supportVertexLocal; +public: + + btTransform m_worldTrans; + btScalar m_maxDot; + btVector3 m_supportVecLocal; + + SupportVertexCallback(const btVector3& supportVecWorld,const btTransform& trans) + : m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f) + + { + m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); + } + + virtual void processTriangle( btVector3* triangle,int partId, int triangleIndex) + { + 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 GetSupportVertexWorldSpace() + { + return m_worldTrans(m_supportVertexLocal); + } + + btVector3 GetSupportVertexLocal() + { + return m_supportVertexLocal; + } + +}; + + +void btTriangleMeshShape::setLocalScaling(const btVector3& scaling) +{ + m_meshInterface->setScaling(scaling); + recalcLocalAabb(); +} + +const btVector3& btTriangleMeshShape::getLocalScaling() const +{ + return m_meshInterface->getScaling(); +} + + + + + + +//#define DEBUG_TRIANGLE_MESH + + +void btTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + + struct FilteredCallback : public btInternalTriangleIndexCallback + { + btTriangleCallback* m_callback; + btVector3 m_aabbMin; + btVector3 m_aabbMax; + + FilteredCallback(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) + :m_callback(callback), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + if (TestTriangleAgainstAabb2(&triangle[0],m_aabbMin,m_aabbMax)) + { + //check aabb in triangle-space, before doing this + m_callback->processTriangle(triangle,partId,triangleIndex); + } + + } + + }; + + FilteredCallback filterCallback(callback,aabbMin,aabbMax); + + m_meshInterface->InternalProcessAllTriangles(&filterCallback,aabbMin,aabbMax); + +} + + + + + +void btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) +{ + //moving concave objects not supported + assert(0); + inertia.setValue(0.f,0.f,0.f); +} + + +btVector3 btTriangleMeshShape::localGetSupportingVertex(const btVector3& vec) const +{ + btVector3 supportVertex; + + btTransform ident; + ident.setIdentity(); + + SupportVertexCallback supportCallback(vec,ident); + + btVector3 aabbMax(1e30f,1e30f,1e30f); + + processAllTriangles(&supportCallback,-aabbMax,aabbMax); + + supportVertex = supportCallback.GetSupportVertexLocal(); + + return supportVertex; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h new file mode 100644 index 00000000000..81cb1412db9 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -0,0 +1,68 @@ +/* +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 TRIANGLE_MESH_SHAPE_H +#define TRIANGLE_MESH_SHAPE_H + +#include "BulletCollision/CollisionShapes/btConcaveShape.h" +#include "BulletCollision/CollisionShapes/btStridingMeshInterface.h" + + +///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. +class btTriangleMeshShape : public ConcaveShape +{ +protected: + btStridingMeshInterface* m_meshInterface; + btVector3 m_localAabbMin; + btVector3 m_localAabbMax; + + +public: + btTriangleMeshShape(btStridingMeshInterface* meshInterface); + + virtual ~btTriangleMeshShape(); + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const; + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const + { + assert(0); + return localGetSupportingVertex(vec); + } + + void recalcLocalAabb(); + + virtual int getShapeType() const + { + return TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + + 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); + + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + + + //debugging + virtual char* getName()const {return "TRIANGLEMESH";} + + +}; + +#endif //TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h new file mode 100644 index 00000000000..8f0a06f7586 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -0,0 +1,164 @@ +/* +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 OBB_TRIANGLE_MINKOWSKI_H +#define OBB_TRIANGLE_MINKOWSKI_H + +#include "btConvexShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" + +class btTriangleShape : public btPolyhedralConvexShape +{ + + +public: + + btVector3 m_vertices1[3]; + + + virtual int getNumVertices() const + { + return 3; + } + + const btVector3& getVertexPtr(int index) const + { + return m_vertices1[index]; + } + virtual void getVertex(int index,btVector3& vert) const + { + vert = m_vertices1[index]; + } + virtual int getShapeType() const + { + return TRIANGLE_SHAPE_PROXYTYPE; + } + + virtual int getNumEdges() const + { + return 3; + } + + virtual void getEdge(int i,btPoint3& pa,btPoint3& pb) const + { + getVertex(i,pa); + getVertex((i+1)%3,pb); + } + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const + { +// ASSERT(0); + getAabbSlow(t,aabbMin,aabbMax); + } + + btVector3 localGetSupportingVertexWithoutMargin(const btVector3& dir)const + { + btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2])); + return m_vertices1[dots.maxAxis()]; + + } + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const + { + for (int i=0;i<numVectors;i++) + { + const btVector3& dir = vectors[i]; + btVector3 dots(dir.dot(m_vertices1[0]), dir.dot(m_vertices1[1]), dir.dot(m_vertices1[2])); + supportVerticesOut[i] = m_vertices1[dots.maxAxis()]; + } + + } + + + + btTriangleShape(const btVector3& p0,const btVector3& p1,const btVector3& p2) + { + m_vertices1[0] = p0; + m_vertices1[1] = p1; + m_vertices1[2] = p2; + } + + + + virtual void getPlane(btVector3& planeNormal,btPoint3& planeSupport,int i) const + { + getPlaneEquation(i,planeNormal,planeSupport); + } + + virtual int getNumPlanes() const + { + return 1; + } + + void calcNormal(btVector3& normal) const + { + normal = (m_vertices1[1]-m_vertices1[0]).cross(m_vertices1[2]-m_vertices1[0]); + normal.normalize(); + } + + virtual void getPlaneEquation(int i, btVector3& planeNormal,btPoint3& planeSupport) const + { + calcNormal(planeNormal); + planeSupport = m_vertices1[0]; + } + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) + { + ASSERT(0); + inertia.setValue(0.f,0.f,0.f); + } + + virtual bool isInside(const btPoint3& pt,btScalar tolerance) const + { + btVector3 normal; + calcNormal(normal); + //distance to plane + btScalar dist = pt.dot(normal); + btScalar planeconst = m_vertices1[0].dot(normal); + dist -= planeconst; + if (dist >= -tolerance && dist <= tolerance) + { + //inside check on edge-planes + int i; + for (i=0;i<3;i++) + { + btPoint3 pa,pb; + getEdge(i,pa,pb); + btVector3 edge = pb-pa; + btVector3 edgeNormal = edge.cross(normal); + edgeNormal.normalize(); + btScalar dist = pt.dot( edgeNormal); + btScalar edgeConst = pa.dot(edgeNormal); + dist -= edgeConst; + if (dist < -tolerance) + return false; + } + + return true; + } + + return false; + } + //debugging + virtual char* getName()const + { + return "Triangle"; + } + + +}; + +#endif //OBB_TRIANGLE_MINKOWSKI_H + diff --git a/extern/bullet2/src/BulletCollision/Doxyfile b/extern/bullet2/src/BulletCollision/Doxyfile new file mode 100644 index 00000000000..4ecb6acb62f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/Doxyfile @@ -0,0 +1,746 @@ +# Doxyfile 1.2.4 + +# This file describes the settings to be used by doxygen for a project +# +# All text after a hash (#) is considered a comment and will be ignored +# The format is: +# TAG = value [value, ...] +# For lists items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (" ") + +#--------------------------------------------------------------------------- +# General configuration options +#--------------------------------------------------------------------------- + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded +# by quotes) that should identify the project. +PROJECT_NAME = "Bullet Continuous Collision Detection Library" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. +# This could be handy for archiving the generated documentation or +# if some version control system is used. + +PROJECT_NUMBER = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) +# base path where the generated documentation will be put. +# If a relative path is entered, it will be relative to the location +# where doxygen was started. If left blank the current directory will be used. + +OUTPUT_DIRECTORY = + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# The default language is English, other supported languages are: +# Dutch, French, Italian, Czech, Swedish, German, Finnish, Japanese, +# Korean, Hungarian, Norwegian, Spanish, Romanian, Russian, Croatian, +# Polish, Portuguese and Slovene. + +OUTPUT_LANGUAGE = English + +# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in +# documentation are documented, even if no documentation was available. +# Private class members and static file members will be hidden unless +# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES all private members of a class +# will be included in the documentation. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_STATIC tag is set to YES all static members of a file +# will be included in the documentation. + +EXTRACT_STATIC = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all +# undocumented members of documented classes, files or namespaces. +# If set to NO (the default) these members will be included in the +# various overviews, but no documentation section is generated. +# This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. +# If set to NO (the default) these class will be included in the various +# overviews. This option has no effect if EXTRACT_ALL is enabled. + +HIDE_UNDOC_CLASSES = NO + +# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will +# include brief member descriptions after the members that are listed in +# the file and class documentation (similar to JavaDoc). +# Set to NO to disable this. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend +# the brief description of a member or function before the detailed description. +# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. + +REPEAT_BRIEF = YES + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# Doxygen will generate a detailed section even if there is only a brief +# description. + +ALWAYS_DETAILED_SEC = NO + +# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full +# path before files name in the file list and in the header files. If set +# to NO the shortest path that makes the file name unique will be used. + +FULL_PATH_NAMES = NO + +# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag +# can be used to strip a user defined part of the path. Stripping is +# only done if one of the specified strings matches the left-hand part of +# the path. It is allowed to use relative paths in the argument list. + +STRIP_FROM_PATH = + +# The INTERNAL_DOCS tag determines if documentation +# that is typed after a \internal command is included. If the tag is set +# to NO (the default) then the documentation will be excluded. +# Set it to YES to include the internal documentation. + +INTERNAL_DOCS = NO + +# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will +# generate a class diagram (in Html and LaTeX) for classes with base or +# super classes. Setting the tag to NO turns the diagrams off. + +CLASS_DIAGRAMS = YES + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will +# be generated. Documented entities will be cross-referenced with these sources. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body +# of functions and classes directly in the documentation. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct +# doxygen to hide any special comment blocks from generated source code +# fragments. Normal C and C++ comments will always remain visible. + +STRIP_CODE_COMMENTS = YES + +# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate +# file names in lower case letters. If set to YES upper case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# users are adviced to set this option to NO. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen +# will show members with their full class and namespace scopes in the +# documentation. If set to YES the scope will be hidden. + +HIDE_SCOPE_NAMES = NO + +# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen +# will generate a verbatim copy of the header file for each class for +# which an include is specified. Set to NO to disable this. + +VERBATIM_HEADERS = YES + +# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen +# will put list of the files that are included by a file in the documentation +# of that file. + +SHOW_INCLUDE_FILES = YES + +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen +# will interpret the first line (until the first dot) of a JavaDoc-style +# comment as the brief description. If set to NO, the JavaDoc +# comments will behave just like the Qt-style comments (thus requiring an +# explict @brief command for a brief description. + +JAVADOC_AUTOBRIEF = YES + +# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented +# member inherits the documentation from any documented member that it +# reimplements. + +INHERIT_DOCS = YES + +# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] +# is inserted in the documentation for inline members. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen +# will sort the (detailed) documentation of file and class members +# alphabetically by member name. If set to NO the members will appear in +# declaration order. + +SORT_MEMBER_DOCS = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES, then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. + +DISTRIBUTE_GROUP_DOC = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. +# Doxygen uses this value to replace tabs by spaces in code fragments. + +TAB_SIZE = 8 + +# The ENABLE_SECTIONS tag can be used to enable conditional +# documentation sections, marked by \if sectionname ... \endif. + +ENABLED_SECTIONS = + +# The GENERATE_TODOLIST tag can be used to enable (YES) or +# disable (NO) the todo list. This list is created by putting \todo +# commands in the documentation. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or +# disable (NO) the test list. This list is created by putting \test +# commands in the documentation. + +GENERATE_TESTLIST = YES + +# This tag can be used to specify a number of aliases that acts +# as commands in the documentation. An alias has the form "name=value". +# For example adding "sideeffect=\par Side Effects:\n" will allow you to +# put the command \sideeffect (or @sideeffect) in the documentation, which +# will result in a user defined paragraph with heading "Side Effects:". +# You can put \n's in the value part of an alias to insert newlines. + +ALIASES = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated +# by doxygen. Possible values are YES and NO. If left blank NO is used. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated by doxygen. Possible values are YES and NO. If left blank +# NO is used. + +WARNINGS = YES + +# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings +# for undocumented members. If EXTRACT_ALL is set to YES then this flag will +# automatically be disabled. + +WARN_IF_UNDOCUMENTED = YES + +# The WARN_FORMAT tag determines the format of the warning messages that +# doxygen can produce. The string should contain the $file, $line, and $text +# tags, which will be replaced by the file and line number from which the +# warning originated and the warning text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning +# and error messages should be written. If left blank the output is written +# to stderr. + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag can be used to specify the files and/or directories that contain +# documented source files. You may enter file names like "myfile.cpp" or +# directories like "/usr/src/myproject". Separate the files or directories +# with spaces. + +INPUT = . + + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +FILE_PATTERNS = *.h *.cpp *.c + +# The RECURSIVE tag can be used to turn specify whether or not subdirectories +# should be searched for input files as well. Possible values are YES and NO. +# If left blank NO is used. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. + +EXCLUDE = + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. + +EXCLUDE_PATTERNS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or +# directories that contain example code fragments that are included (see +# the \include command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp +# and *.h) to filter out the source-files in the directories. If left +# blank all files are included. + +EXAMPLE_PATTERNS = + +# The IMAGE_PATH tag can be used to specify one or more files or +# directories that contain image that are included in the documentation (see +# the \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command <filter> <input-file>, where <filter> +# is the value of the INPUT_FILTER tag, and <input-file> is the name of an +# input file. Doxygen will then use the output that the filter program writes +# to standard output. + +INPUT_FILTER = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will be used to filter the input files when producing source +# files to browse. + +FILTER_SOURCE_FILES = NO + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index +# of all compounds will be generated. Enable this if the project +# contains a lot of classes, structs, unions or interfaces. + +ALPHABETICAL_INDEX = NO + +# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then +# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns +# in which this list will be split (can be a number in the range [1..20]) + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all +# classes will be put under the same header in the alphabetical index. +# The IGNORE_PREFIX tag can be used to specify one or more prefixes that +# should be ignored while generating the index headers. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES (the default) Doxygen will +# generate HTML output. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `html' will be used as the default path. + +HTML_OUTPUT = html + +# The HTML_HEADER tag can be used to specify a personal HTML header for +# each generated HTML page. If it is left blank doxygen will generate a +# standard header. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a personal HTML footer for +# each generated HTML page. If it is left blank doxygen will generate a +# standard footer. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user defined cascading +# style sheet that is used by each HTML page. It can be used to +# fine-tune the look of the HTML output. If the tag is left blank doxygen +# will generate a default style sheet + +HTML_STYLESHEET = + +# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, +# files or namespaces will be aligned in HTML using tables. If set to +# NO a bullet list will be used. + +HTML_ALIGN_MEMBERS = YES + +# If the GENERATE_HTMLHELP tag is set to YES, additional index files +# will be generated that can be used as input for tools like the +# Microsoft HTML help workshop to generate a compressed HTML help file (.chm) +# of the generated HTML documentation. + +GENERATE_HTMLHELP = NO + +# The DISABLE_INDEX tag can be used to turn on/off the condensed index at +# top of each HTML page. The value NO (the default) enables the index and +# the value YES disables it. + +DISABLE_INDEX = NO + +# This tag can be used to set the number of enum values (range [1..20]) +# that doxygen will group on one line in the generated HTML documentation. + +ENUM_VALUES_PER_LINE = 4 + +# If the GENERATE_TREEVIEW tag is set to YES, a side pannel will be +# generated containing a tree-like index structure (just like the one that +# is generated for HTML Help). For this to work a browser that supports +# JavaScript and frames is required (for instance Netscape 4.0+ +# or Internet explorer 4.0+). + +GENERATE_TREEVIEW = NO + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be +# used to set the initial width (in pixels) of the frame in which the tree +# is shown. + +TREEVIEW_WIDTH = 250 + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- + +# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will +# generate Latex output. + +GENERATE_LATEX = NO + +# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `latex' will be used as the default path. + +LATEX_OUTPUT = latex + +# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact +# LaTeX documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_LATEX = NO + +# The PAPER_TYPE tag can be used to set the paper type that is used +# by the printer. Possible values are: a4, a4wide, letter, legal and +# executive. If left blank a4wide will be used. + +PAPER_TYPE = a4wide + +# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX +# packages that should be included in the LaTeX output. + +EXTRA_PACKAGES = + +# The LATEX_HEADER tag can be used to specify a personal LaTeX header for +# the generated latex document. The header should contain everything until +# the first chapter. If it is left blank doxygen will generate a +# standard header. Notice: only use this tag if you know what you are doing! + +LATEX_HEADER = + +# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated +# is prepared for conversion to pdf (using ps2pdf). The pdf file will +# contain links (just like the HTML output) instead of page references +# This makes the output suitable for online browsing using a pdf viewer. + +PDF_HYPERLINKS = NO + +# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of +# plain latex in the generated Makefile. Set this option to YES to get a +# higher quality PDF documentation. + +USE_PDFLATEX = NO + +# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. +# command to the generated LaTeX files. This will instruct LaTeX to keep +# running if errors occur, instead of asking the user for help. +# This option is also used when generating formulas in HTML. + +LATEX_BATCHMODE = NO + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- + +# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output +# The RTF output is optimised for Word 97 and may not look very pretty with +# other RTF readers or editors. + +GENERATE_RTF = NO + +# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `rtf' will be used as the default path. + +RTF_OUTPUT = rtf + +# If the COMPACT_RTF tag is set to YES Doxygen generates more compact +# RTF documents. This may be useful for small projects and may help to +# save some trees in general. + +COMPACT_RTF = NO + +# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated +# will contain hyperlink fields. The RTF file will +# contain links (just like the HTML output) instead of page references. +# This makes the output suitable for online browsing using a WORD or other. +# programs which support those fields. +# Note: wordpad (write) and others do not support links. + +RTF_HYPERLINKS = NO + +# Load stylesheet definitions from file. Syntax is similar to doxygen's +# config file, i.e. a series of assigments. You only have to provide +# replacements, missing definitions are set to their default value. + +RTF_STYLESHEET_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- + +# If the GENERATE_MAN tag is set to YES (the default) Doxygen will +# generate man pages + +GENERATE_MAN = NO + +# The MAN_OUTPUT tag is used to specify where the man pages will be put. +# If a relative path is entered the value of OUTPUT_DIRECTORY will be +# put in front of it. If left blank `man' will be used as the default path. + +MAN_OUTPUT = man + +# The MAN_EXTENSION tag determines the extension that is added to +# the generated man pages (default is the subroutine's section .3) + +MAN_EXTENSION = .3 + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- + +# If the GENERATE_XML tag is set to YES Doxygen will +# generate an XML file that captures the structure of +# the code including all documentation. Warning: This feature +# is still experimental and very incomplete. + +GENERATE_XML = NO + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- + +# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will +# evaluate all C-preprocessor directives found in the sources and include +# files. + +ENABLE_PREPROCESSING = YES + +# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro +# names in the source code. If set to NO (the default) only conditional +# compilation will be performed. Macro expansion can be done in a controlled +# way by setting EXPAND_ONLY_PREDEF to YES. + +MACRO_EXPANSION = NO + +# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES +# then the macro expansion is limited to the macros specified with the +# PREDEFINED and EXPAND_AS_PREDEFINED tags. + +EXPAND_ONLY_PREDEF = NO + +# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files +# in the INCLUDE_PATH (see below) will be search if a #include is found. + +SEARCH_INCLUDES = YES + +# The INCLUDE_PATH tag can be used to specify one or more directories that +# contain include files that are not input files but should be processed by +# the preprocessor. + +INCLUDE_PATH = ../../generic/extern + +# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard +# patterns (like *.h and *.hpp) to filter out the header-files in the +# directories. If left blank, the patterns specified with FILE_PATTERNS will +# be used. + +INCLUDE_FILE_PATTERNS = + +# The PREDEFINED tag can be used to specify one or more macro names that +# are defined before the preprocessor is started (similar to the -D option of +# gcc). The argument of the tag is a list of macros of the form: name +# or name=definition (no spaces). If the definition and the = are +# omitted =1 is assumed. + +PREDEFINED = + +# If the MACRO_EXPANSION and EXPAND_PREDEF_ONLY tags are set to YES then +# this tag can be used to specify a list of macro names that should be expanded. +# The macro definition that is found in the sources will be used. +# Use the PREDEFINED tag if you want to use a different macro definition. + +EXPAND_AS_DEFINED = + +#--------------------------------------------------------------------------- +# Configuration::addtions related to external references +#--------------------------------------------------------------------------- + +# The TAGFILES tag can be used to specify one or more tagfiles. + +TAGFILES = + +# When a file name is specified after GENERATE_TAGFILE, doxygen will create +# a tag file that is based on the input files it reads. + +GENERATE_TAGFILE = + +# If the ALLEXTERNALS tag is set to YES all external classes will be listed +# in the class index. If set to NO only the inherited external classes +# will be listed. + +ALLEXTERNALS = NO + +# The PERL_PATH should be the absolute path and name of the perl script +# interpreter (i.e. the result of `which perl'). + +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- + +# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# available from the path. This tool is part of Graphviz, a graph visualization +# toolkit from AT&T and Lucent Bell Labs. The other options in this section +# have no effect if this option is set to NO (the default) + +HAVE_DOT = YES + +# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect inheritance relations. Setting this tag to YES will force the +# the CLASS_DIAGRAMS tag to NO. + +CLASS_GRAPH = YES + +# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen +# will generate a graph for each documented class showing the direct and +# indirect implementation dependencies (inheritance, containment, and +# class references variables) of the class with other documented classes. + +COLLABORATION_GRAPH = YES + +# If the ENABLE_PREPROCESSING, INCLUDE_GRAPH, and HAVE_DOT tags are set to +# YES then doxygen will generate a graph for each documented file showing +# the direct and indirect include dependencies of the file with other +# documented files. + +INCLUDE_GRAPH = YES + +# If the ENABLE_PREPROCESSING, INCLUDED_BY_GRAPH, and HAVE_DOT tags are set to +# YES then doxygen will generate a graph for each documented header file showing +# the documented files that directly or indirectly include this file + +INCLUDED_BY_GRAPH = YES + +# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen +# will graphical hierarchy of all classes instead of a textual one. + +GRAPHICAL_HIERARCHY = YES + +# The tag DOT_PATH can be used to specify the path where the dot tool can be +# found. If left blank, it is assumed the dot tool can be found on the path. + +DOT_PATH = + +# The MAX_DOT_GRAPH_WIDTH tag can be used to set the maximum allowed width +# (in pixels) of the graphs generated by dot. If a graph becomes larger than +# this value, doxygen will try to truncate the graph, so that it fits within +# the specified constraint. Beware that most browsers cannot cope with very +# large images. + +MAX_DOT_GRAPH_WIDTH = 1024 + +# The MAX_DOT_GRAPH_HEIGHT tag can be used to set the maximum allows height +# (in pixels) of the graphs generated by dot. If a graph becomes larger than +# this value, doxygen will try to truncate the graph, so that it fits within +# the specified constraint. Beware that most browsers cannot cope with very +# large images. + +MAX_DOT_GRAPH_HEIGHT = 1024 + +# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will +# generate a legend page explaining the meaning of the various boxes and +# arrows in the dot generated graphs. + +GENERATE_LEGEND = YES + +#--------------------------------------------------------------------------- +# Configuration::addtions related to the search engine +#--------------------------------------------------------------------------- + +# The SEARCHENGINE tag specifies whether or not a search engine should be +# used. If set to NO the values of all tags below this one will be ignored. + +SEARCHENGINE = NO + +# The CGI_NAME tag should be the name of the CGI script that +# starts the search engine (doxysearch) with the correct parameters. +# A script with this name will be generated by doxygen. + +CGI_NAME = search.cgi + +# The CGI_URL tag should be the absolute URL to the directory where the +# cgi binaries are located. See the documentation of your http daemon for +# details. + +CGI_URL = + +# The DOC_URL tag should be the absolute URL to the directory where the +# documentation is located. If left blank the absolute path to the +# documentation, with file:// prepended to it, will be used. + +DOC_URL = + +# The DOC_ABSPATH tag should be the absolute path to the directory where the +# documentation is located. If left blank the directory on the local machine +# will be used. + +DOC_ABSPATH = + +# The BIN_ABSPATH tag must point to the directory where the doxysearch binary +# is installed. + +BIN_ABSPATH = c:\program files\doxygen\bin + +# The EXT_DOC_PATHS tag can be used to specify one or more paths to +# documentation generated for other projects. This allows doxysearch to search +# the documentation for these projects as well. + +EXT_DOC_PATHS = diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp new file mode 100644 index 00000000000..ae3ce42e77f --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -0,0 +1,200 @@ +/* +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 "btContinuousConvexCollision.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "btGjkPairDetector.h" +#include "btPointCollector.h" + + + +btContinuousConvexCollision::btContinuousConvexCollision ( btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_simplexSolver(simplexSolver), +m_penetrationDepthSolver(penetrationDepthSolver), +m_convexA(convexA),m_convexB(convexB) +{ +} + +/// This maximum should not be necessary. It allows for untested/degenerate cases in production code. +/// You don't want your game ever to lock-up. +#define MAX_ITERATIONS 1000 + +bool btContinuousConvexCollision::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ + + m_simplexSolver->reset(); + + /// compute linear and angular velocity for this interval, to interpolate + btVector3 linVelA,angVelA,linVelB,angVelB; + btTransformUtil::calculateVelocity(fromA,toA,1.f,linVelA,angVelA); + btTransformUtil::calculateVelocity(fromB,toB,1.f,linVelB,angVelB); + + btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); + btScalar boundingRadiusB = m_convexB->getAngularMotionDisc(); + + btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; + + float radius = 0.001f; + + btScalar lambda = 0.f; + btVector3 v(1,0,0); + + int maxIter = MAX_ITERATIONS; + + btVector3 n; + n.setValue(0.f,0.f,0.f); + bool hasResult = false; + btVector3 c; + + float lastLambda = lambda; + //float epsilon = 0.001f; + + int numIter = 0; + //first solution, using GJK + + + btTransform identityTrans; + identityTrans.setIdentity(); + + btSphereShape raySphere(0.0f); + raySphere.setMargin(0.f); + + +// result.drawCoordSystem(sphereTr); + + btPointCollector pointCollector1; + + { + + btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; + + //we don't use margins during CCD + gjk.setIgnoreMargin(true); + + input.m_transformA = fromA; + input.m_transformB = fromB; + gjk.getClosestPoints(input,pointCollector1,0); + + hasResult = pointCollector1.m_hasResult; + c = pointCollector1.m_pointInWorld; + } + + if (hasResult) + { + btScalar dist; + dist = pointCollector1.m_distance; + n = pointCollector1.m_normalOnBInWorld; + + //not close enough + while (dist > radius) + { + numIter++; + if (numIter > maxIter) + return false; //todo: report a failure + + float dLambda = 0.f; + + //calculate safe moving fraction from distance / (linear+rotational velocity) + + //float clippedDist = GEN_min(angularConservativeRadius,dist); + //float clippedDist = dist; + + float projectedLinearVelocity = (linVelB-linVelA).dot(n); + + dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); + + lambda = lambda + dLambda; + + if (lambda > 1.f) + return false; + + if (lambda < 0.f) + return false; + + //todo: next check with relative epsilon + if (lambda <= lastLambda) + break; + lastLambda = lambda; + + + + //interpolate to next lambda + btTransform interpolatedTransA,interpolatedTransB,relativeTrans; + + btTransformUtil::integrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); + btTransformUtil::integrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); + relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); + + result.DebugDraw( lambda ); + + btPointCollector pointCollector; + btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; + input.m_transformA = interpolatedTransA; + input.m_transformB = interpolatedTransB; + gjk.getClosestPoints(input,pointCollector,0); + if (pointCollector.m_hasResult) + { + if (pointCollector.m_distance < 0.f) + { + //degenerate ?! + result.m_fraction = lastLambda; + result.m_normal = n; + return true; + } + c = pointCollector.m_pointInWorld; + + dist = pointCollector.m_distance; + } else + { + //?? + return false; + } + + } + + result.m_fraction = lambda; + result.m_normal = n; + return true; + } + + return false; + +/* +//todo: + //if movement away from normal, discard result + btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); + if (result.m_fraction < 1.f) + { + if (move.dot(result.m_normal) <= 0.f) + { + } + } +*/ + +} + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h new file mode 100644 index 00000000000..9901bab4b45 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -0,0 +1,52 @@ +/* +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 CONTINUOUS_COLLISION_CONVEX_CAST_H +#define CONTINUOUS_COLLISION_CONVEX_CAST_H + +#include "btConvexCast.h" +#include "btSimplexSolverInterface.h" +class btConvexPenetrationDepthSolver; +class btConvexShape; + +/// btContinuousConvexCollision implements angular and linear time of impact for convex objects. +/// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). +/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. +/// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops +class btContinuousConvexCollision : public btConvexCast +{ + btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btConvexShape* m_convexA; + btConvexShape* m_convexB; + + +public: + + btContinuousConvexCollision (btConvexShape* shapeA,btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); + + +}; + +#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp new file mode 100644 index 00000000000..d2a1310b232 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.cpp @@ -0,0 +1,20 @@ +/* +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 "btConvexCast.h" + +btConvexCast::~btConvexCast() +{ +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h new file mode 100644 index 00000000000..4258d829cca --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -0,0 +1,71 @@ +/* +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_CAST_H +#define CONVEX_CAST_H + +#include <LinearMath/btTransform.h> +#include <LinearMath/btVector3.h> +#include <LinearMath/btScalar.h> +class btMinkowskiSumShape; +#include "LinearMath/btIDebugDraw.h" + +/// btConvexCast is an interface for Casting +class btConvexCast +{ +public: + + + virtual ~btConvexCast(); + + ///RayResult stores the closest result + /// alternatively, add a callback method to decide about closest/all results + struct CastResult + { + //virtual bool addRayResult(const btVector3& normal,btScalar fraction) = 0; + + virtual void DebugDraw(btScalar fraction) {} + virtual void drawCoordSystem(const btTransform& trans) {} + + CastResult() + :m_fraction(1e30f), + m_debugDrawer(0) + { + } + + + virtual ~CastResult() {}; + + btVector3 m_normal; + btScalar m_fraction; + btTransform m_hitTransformA; + btTransform m_hitTransformB; + + btIDebugDraw* m_debugDrawer; + + }; + + + /// cast a convex against another convex object + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) = 0; +}; + +#endif //CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h new file mode 100644 index 00000000000..ba02ea56e83 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -0,0 +1,42 @@ +/* +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_PENETRATION_DEPTH_H +#define CONVEX_PENETRATION_DEPTH_H + +class btVector3; +#include "btSimplexSolverInterface.h" +class btConvexShape; +#include "LinearMath/btPoint3.h" +class btTransform; + +///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. +class btConvexPenetrationDepthSolver +{ +public: + + virtual ~btConvexPenetrationDepthSolver() {}; + virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, + btConvexShape* convexA,btConvexShape* convexB, + const btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw + ) = 0; + + +}; +#endif //CONVEX_PENETRATION_DEPTH_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h new file mode 100644 index 00000000000..8889699b395 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -0,0 +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. +*/ + + +#ifndef DISCRETE_COLLISION_DETECTOR_INTERFACE_H +#define DISCRETE_COLLISION_DETECTOR_INTERFACE_H +#include "LinearMath/btTransform.h" +#include "LinearMath/btVector3.h" + + +/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations +/// This interface allows to query for closest points and penetration depth between two (convex) objects +/// the closest point is on the second object (B), and the normal points from the surface on B towards A. +/// distance is between closest points on B and closest point on A. So you can calculate closest point on A +/// by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB +struct btDiscreteCollisionDetectorInterface +{ + + struct Result + { + void operator delete(void* ptr) {}; + + virtual ~Result(){} + + ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0; + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth)=0; + }; + + struct ClosestPointInput + { + ClosestPointInput() + :m_maximumDistanceSquared(1e30f) + { + } + + btTransform m_transformA; + btTransform m_transformB; + btScalar m_maximumDistanceSquared; + }; + + virtual ~btDiscreteCollisionDetectorInterface() {}; + + // + // give either closest points (distance > 0) or penetration (distance) + // the normal always points from B towards A + // + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) = 0; + +}; + +struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result +{ + btVector3 m_normalOnSurfaceB; + btVector3 m_closestPointInB; + btScalar m_distance; //negative means penetration ! + + btStorageResult() : m_distance(1e30f) + { + + } + virtual ~btStorageResult() {}; + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + { + if (depth < m_distance) + { + m_normalOnSurfaceB = normalOnBInWorld; + m_closestPointInB = pointInWorld; + m_distance = depth; + } + } +}; + +#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp new file mode 100644 index 00000000000..bf465b61857 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp @@ -0,0 +1,174 @@ +/* +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 "btGjkConvexCast.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "btGjkPairDetector.h" +#include "btPointCollector.h" + + +btGjkConvexCast::btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) +:m_simplexSolver(simplexSolver), +m_convexA(convexA), +m_convexB(convexB) +{ +} + +bool btGjkConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ + + + btMinkowskiSumShape combi(m_convexA,m_convexB); + btMinkowskiSumShape* convex = &combi; + + btTransform rayFromLocalA; + btTransform rayToLocalA; + + rayFromLocalA = fromA.inverse()* fromB; + rayToLocalA = toA.inverse()* toB; + + + btTransform trA,trB; + trA = btTransform(fromA); + trB = btTransform(fromB); + trA.setOrigin(btPoint3(0,0,0)); + trB.setOrigin(btPoint3(0,0,0)); + + convex->setTransformA(trA); + convex->setTransformB(trB); + + + + + float radius = 0.01f; + + btScalar lambda = 0.f; + btVector3 s = rayFromLocalA.getOrigin(); + btVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); + btVector3 x = s; + btVector3 n; + n.setValue(0,0,0); + bool hasResult = false; + btVector3 c; + + float lastLambda = lambda; + + //first solution, using GJK + + //no penetration support for now, perhaps pass a pointer when we really want it + btConvexPenetrationDepthSolver* penSolverPtr = 0; + + btTransform identityTrans; + identityTrans.setIdentity(); + + btSphereShape raySphere(0.0f); + raySphere.setMargin(0.f); + + btTransform sphereTr; + sphereTr.setIdentity(); + sphereTr.setOrigin( rayFromLocalA.getOrigin()); + + result.drawCoordSystem(sphereTr); + { + btPointCollector pointCollector1; + btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + + btGjkPairDetector::ClosestPointInput input; + input.m_transformA = sphereTr; + input.m_transformB = identityTrans; + gjk.getClosestPoints(input,pointCollector1,0); + + hasResult = pointCollector1.m_hasResult; + c = pointCollector1.m_pointInWorld; + n = pointCollector1.m_normalOnBInWorld; + } + + + + if (hasResult) + { + btScalar dist; + dist = (c-x).length(); + if (dist < radius) + { + //penetration + lastLambda = 1.f; + } + + //not close enough + while (dist > radius) + { + + n = x - c; + btScalar nDotr = n.dot(r); + + if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON)) + return false; + + lambda = lambda - n.dot(n) / nDotr; + if (lambda <= lastLambda) + break; + + lastLambda = lambda; + + x = s + lambda * r; + + sphereTr.setOrigin( x ); + result.drawCoordSystem(sphereTr); + btPointCollector pointCollector; + btGjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + btGjkPairDetector::ClosestPointInput input; + input.m_transformA = sphereTr; + input.m_transformB = identityTrans; + gjk.getClosestPoints(input,pointCollector,0); + if (pointCollector.m_hasResult) + { + if (pointCollector.m_distance < 0.f) + { + //degeneracy, report a hit + result.m_fraction = lastLambda; + result.m_normal = n; + return true; + } + c = pointCollector.m_pointInWorld; + dist = (c-x).length(); + } else + { + //?? + return false; + } + + } + + if (lastLambda < 1.f) + { + + result.m_fraction = lastLambda; + result.m_normal = n; + return true; + } + } + + return false; +} + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h new file mode 100644 index 00000000000..66b34b88363 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -0,0 +1,50 @@ +/* +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 GJK_CONVEX_CAST_H +#define GJK_CONVEX_CAST_H + +#include <BulletCollision/CollisionShapes/btCollisionMargin.h> + +#include "LinearMath/btVector3.h" +#include "btConvexCast.h" +class btConvexShape; +class btMinkowskiSumShape; +#include "btSimplexSolverInterface.h" + +///GjkConvexCast performs a raycast on a convex object using support mapping. +class btGjkConvexCast : public btConvexCast +{ + btSimplexSolverInterface* m_simplexSolver; + btConvexShape* m_convexA; + btConvexShape* m_convexB; + +public: + + btGjkConvexCast(btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver); + + /// cast a convex against another convex object + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); + +}; + +#endif //GJK_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp new file mode 100644 index 00000000000..f6fdd6435cf --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -0,0 +1,218 @@ +/* +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 "btGjkPairDetector.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" + +#if defined(DEBUG) || defined (_DEBUG) +#include <stdio.h> //for debug printf +#endif + +static const btScalar rel_error = btScalar(1.0e-5); +btScalar rel_error2 = rel_error * rel_error; +float maxdist2 = 1.e30f; + +#ifdef __SPU__ +#include <spu_printf.h> +#endif //__SPU__ + +btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_cachedSeparatingAxis(0.f,0.f,1.f), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_ignoreMargin(false), +m_partId0(-1), +m_index0(-1), +m_partId1(-1), +m_index1(-1) +{ +} + +void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) +{ + btScalar distance=0.f; + btVector3 normalInB(0.f,0.f,0.f); + btVector3 pointOnA,pointOnB; + + float marginA = m_minkowskiA->getMargin(); + float marginB = m_minkowskiB->getMargin(); + + //for CCD we don't use margins + if (m_ignoreMargin) + { + marginA = 0.f; + marginB = 0.f; + } + +int curIter = 0; + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + + { + btScalar squaredDistance = SIMD_INFINITY; + btScalar delta = 0.f; + + btScalar margin = marginA + marginB; + + + + m_simplexSolver->reset(); + + while (true) + { + + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + + btVector3 pInA = m_minkowskiA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + btPoint3 pWorld = input.m_transformA(pInA); + btPoint3 qWorld = input.m_transformB(qInB); + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) + { + checkPenetration = false; + break; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if (m_simplexSolver->inSimplex(w)) + { + checkSimplex = true; + break; + } + // are we getting any closer ? + if (squaredDistance - delta <= squaredDistance * rel_error2) + { + checkSimplex = true; + break; + } + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + + //calculate the closest point to the origin (update vector v) + if (!m_simplexSolver->closest(m_cachedSeparatingAxis)) + { + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = m_cachedSeparatingAxis.length2(); + + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) + { + m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + checkSimplex = true; + break; + } + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? + m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + break; + } + } + + if (checkSimplex) + { + m_simplexSolver->compute_points(pointOnA, pointOnB); + normalInB = pointOnA-pointOnB; + float lenSqr = m_cachedSeparatingAxis.length2(); + //valid normal + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + float rlen = 1.f / btSqrt(lenSqr ); + normalInB *= rlen; //normalize + btScalar s = btSqrt(squaredDistance); + ASSERT(s > btScalar(0.0)); + pointOnA -= m_cachedSeparatingAxis * (marginA / s); + pointOnB += m_cachedSeparatingAxis * (marginB / s); + distance = ((1.f/rlen) - margin); + isValid = true; + } + } + + if (checkPenetration && !isValid) + { + //penetration case + + //if there is no way to handle penetrations, bail out + if (m_penetrationDepthSolver) + { + // Penetration depth case. + isValid = m_penetrationDepthSolver->calcPenDepth( + *m_simplexSolver, + m_minkowskiA,m_minkowskiB, + input.m_transformA,input.m_transformB, + m_cachedSeparatingAxis, pointOnA, pointOnB, + debugDraw + ); + + if (isValid) + { + normalInB = pointOnB-pointOnA; + float lenSqr = normalInB.length2(); + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) + { + normalInB /= btSqrt(lenSqr); + distance = -(pointOnA-pointOnB).length(); + } else + { + isValid = false; + } + } + } + } + } + + if (isValid) + { +#ifdef __SPU__ + //spu_printf("distance\n"); +#endif //__CELLOS_LV2__ + + output.setShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1); + + output.addContactPoint( + normalInB, + pointOnB, + distance); + //printf("gjk add:%f",distance); + } + + +} + + + + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h new file mode 100644 index 00000000000..bccb0542370 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -0,0 +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 GJK_PAIR_DETECTOR_H +#define GJK_PAIR_DETECTOR_H + +#include "btDiscreteCollisionDetectorInterface.h" +#include "LinearMath/btPoint3.h" + +#include <BulletCollision/CollisionShapes/btCollisionMargin.h> + +class btConvexShape; +#include "btSimplexSolverInterface.h" +class btConvexPenetrationDepthSolver; + +/// btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface +class btGjkPairDetector : public btDiscreteCollisionDetectorInterface +{ + + + btVector3 m_cachedSeparatingAxis; + btConvexPenetrationDepthSolver* m_penetrationDepthSolver; + btSimplexSolverInterface* m_simplexSolver; + btConvexShape* m_minkowskiA; + btConvexShape* m_minkowskiB; + bool m_ignoreMargin; + + +public: + + //experimental feature information, per triangle, per convex etc. + //'material combiner' / contact added callback + int m_partId0; + int m_index0; + int m_partId1; + int m_index1; + + btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~btGjkPairDetector() {}; + + virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw); + + void setMinkowskiA(btConvexShape* minkA) + { + m_minkowskiA = minkA; + } + + void setMinkowskiB(btConvexShape* minkB) + { + m_minkowskiB = minkB; + } + void setCachedSeperatingAxis(const btVector3& seperatingAxis) + { + m_cachedSeparatingAxis = seperatingAxis; + } + + void setPenetrationDepthSolver(btConvexPenetrationDepthSolver* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } + + void setIgnoreMargin(bool ignoreMargin) + { + m_ignoreMargin = ignoreMargin; + } + +}; + +#endif //GJK_PAIR_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h new file mode 100644 index 00000000000..00a9206fef0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -0,0 +1,98 @@ +/* +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 MANIFOLD_CONTACT_POINT_H +#define MANIFOLD_CONTACT_POINT_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransformUtil.h" + + + + + +/// ManifoldContactPoint collects and maintains persistent contactpoints. +/// used to improve stability and performance of rigidbody dynamics response. +class btManifoldPoint + { + public: + btManifoldPoint() + :m_userPersistentData(0) + { + } + + btManifoldPoint( const btVector3 &pointA, const btVector3 &pointB, + const btVector3 &normal, + btScalar distance ) : + m_localPointA( pointA ), + m_localPointB( pointB ), + m_normalWorldOnB( normal ), + m_distance1( distance ), + m_combinedFriction(0.f), + m_combinedRestitution(0.f), + m_userPersistentData(0), + m_lifeTime(0) + { + + + } + + + + btVector3 m_localPointA; + btVector3 m_localPointB; + btVector3 m_positionWorldOnB; + ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity + btVector3 m_positionWorldOnA; + btVector3 m_normalWorldOnB; + + float m_distance1; + float m_combinedFriction; + float m_combinedRestitution; + + + void* m_userPersistentData; + + int m_lifeTime;//lifetime of the contactpoint in frames + + float getDistance() const + { + return m_distance1; + } + int getLifeTime() const + { + return m_lifeTime; + } + + btVector3 getPositionWorldOnA() { + return m_positionWorldOnA; +// return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + const btVector3& getPositionWorldOnB() + { + return m_positionWorldOnB; + } + + void setDistance(float dist) + { + m_distance1 = dist; + } + + + + }; + +#endif //MANIFOLD_CONTACT_POINT_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp new file mode 100644 index 00000000000..34daacf26ac --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -0,0 +1,246 @@ +/* +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 "btMinkowskiPenetrationDepthSolver.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" + + +struct MyResult : public btDiscreteCollisionDetectorInterface::Result +{ + + MyResult():m_hasResult(false) + { + } + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + float m_depth; + bool m_hasResult; + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + } + void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } +}; + +#define NUM_UNITSPHERE_POINTS 42 +static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] = +{ +btVector3(0.000000f , -0.000000f,-1.000000f), +btVector3(0.723608f , -0.525725f,-0.447219f), +btVector3(-0.276388f , -0.850649f,-0.447219f), +btVector3(-0.894426f , -0.000000f,-0.447216f), +btVector3(-0.276388f , 0.850649f,-0.447220f), +btVector3(0.723608f , 0.525725f,-0.447219f), +btVector3(0.276388f , -0.850649f,0.447220f), +btVector3(-0.723608f , -0.525725f,0.447219f), +btVector3(-0.723608f , 0.525725f,0.447219f), +btVector3(0.276388f , 0.850649f,0.447219f), +btVector3(0.894426f , 0.000000f,0.447216f), +btVector3(-0.000000f , 0.000000f,1.000000f), +btVector3(0.425323f , -0.309011f,-0.850654f), +btVector3(-0.162456f , -0.499995f,-0.850654f), +btVector3(0.262869f , -0.809012f,-0.525738f), +btVector3(0.425323f , 0.309011f,-0.850654f), +btVector3(0.850648f , -0.000000f,-0.525736f), +btVector3(-0.525730f , -0.000000f,-0.850652f), +btVector3(-0.688190f , -0.499997f,-0.525736f), +btVector3(-0.162456f , 0.499995f,-0.850654f), +btVector3(-0.688190f , 0.499997f,-0.525736f), +btVector3(0.262869f , 0.809012f,-0.525738f), +btVector3(0.951058f , 0.309013f,0.000000f), +btVector3(0.951058f , -0.309013f,0.000000f), +btVector3(0.587786f , -0.809017f,0.000000f), +btVector3(0.000000f , -1.000000f,0.000000f), +btVector3(-0.587786f , -0.809017f,0.000000f), +btVector3(-0.951058f , -0.309013f,-0.000000f), +btVector3(-0.951058f , 0.309013f,-0.000000f), +btVector3(-0.587786f , 0.809017f,-0.000000f), +btVector3(-0.000000f , 1.000000f,-0.000000f), +btVector3(0.587786f , 0.809017f,-0.000000f), +btVector3(0.688190f , -0.499997f,0.525736f), +btVector3(-0.262869f , -0.809012f,0.525738f), +btVector3(-0.850648f , 0.000000f,0.525736f), +btVector3(-0.262869f , 0.809012f,0.525738f), +btVector3(0.688190f , 0.499997f,0.525736f), +btVector3(0.525730f , 0.000000f,0.850652f), +btVector3(0.162456f , -0.499995f,0.850654f), +btVector3(-0.425323f , -0.309011f,0.850654f), +btVector3(-0.425323f , 0.309011f,0.850654f), +btVector3(0.162456f , 0.499995f,0.850654f) +}; + + +bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, + btConvexShape* convexA,btConvexShape* convexB, + const btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw + ) +{ + + //just take fixed number of orientation, and sample the penetration depth in that direction + float minProj = 1e30f; + btVector3 minNorm; + btVector3 minVertex; + btVector3 minA,minB; + btVector3 seperatingAxisInA,seperatingAxisInB; + btVector3 pInA,qInB,pWorld,qWorld,w; + +#define USE_BATCHED_SUPPORT 1 +#ifdef USE_BATCHED_SUPPORT + btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS]; + btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS]; + btVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS]; + btVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS]; + int i; + + for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + const btVector3& norm = sPenetrationDirections[i]; + seperatingAxisInABatch[i] = (-norm)* transA.getBasis(); + seperatingAxisInBBatch[i] = norm * transB.getBasis(); + } + + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS); + convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS); + for (i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + const btVector3& norm = sPenetrationDirections[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; + + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + w = qWorld - pWorld; + float delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } +#else + for (int i=0;i<NUM_UNITSPHERE_POINTS;i++) + { + const btVector3& norm = sPenetrationDirections[i]; + seperatingAxisInA = (-norm)* transA.getBasis(); + seperatingAxisInB = norm* transB.getBasis(); + pInA = convexA->localGetSupportingVertexWithoutMargin(seperatingAxisInA); + qInB = convexB->localGetSupportingVertexWithoutMargin(seperatingAxisInB); + pWorld = transA(pInA); + qWorld = transB(qInB); + w = qWorld - pWorld; + float delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } + } +#endif //USE_BATCHED_SUPPORT + + //add the margins + + minA += minNorm*convexA->getMargin(); + minB -= minNorm*convexB->getMargin(); + minProj += (convexA->getMargin() + convexB->getMargin()); + + + + +//#define DEBUG_DRAW 1 +#ifdef DEBUG_DRAW + if (debugDraw) + { + btVector3 color(0,1,0); + debugDraw->drawLine(minA,minB,color); + color = btVector3 (1,1,1); + btVector3 vec = minB-minA; + float prj2 = minNorm.dot(vec); + debugDraw->drawLine(minA,minA+(minNorm*minProj),color); + + } +#endif //DEBUG_DRAW + + + + btGjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + + btScalar offsetDist = minProj; + btVector3 offset = minNorm * offsetDist; + + + + btGjkPairDetector::ClosestPointInput input; + + btVector3 newOrg = transA.getOrigin() + offset; + + btTransform displacedTrans = transA; + displacedTrans.setOrigin(newOrg); + + input.m_transformA = displacedTrans; + input.m_transformB = transB; + input.m_maximumDistanceSquared = 1e30f;//minProj; + + MyResult res; + gjkdet.getClosestPoints(input,res,debugDraw); + + float correctedMinNorm = minProj - res.m_depth; + + + //the penetration depth is over-estimated, relax it + float penetration_relaxation= 1.f; + minNorm*=penetration_relaxation; + + if (res.m_hasResult) + { + + pa = res.m_pointInWorld - minNorm * correctedMinNorm; + pb = res.m_pointInWorld; + +#ifdef DEBUG_DRAW + if (debugDraw) + { + btVector3 color(1,0,0); + debugDraw->drawLine(pa,pb,color); + } +#endif//DEBUG_DRAW + + + } + return res.m_hasResult; +} + + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h new file mode 100644 index 00000000000..c287195317e --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -0,0 +1,37 @@ +/* +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 MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + +#include "btConvexPenetrationDepthSolver.h" + +///MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation. +///Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points. +class btMinkowskiPenetrationDepthSolver : public btConvexPenetrationDepthSolver +{ +public: + + virtual bool calcPenDepth( btSimplexSolverInterface& simplexSolver, + btConvexShape* convexA,btConvexShape* convexB, + const btTransform& transA,const btTransform& transB, + btVector3& v, btPoint3& pa, btPoint3& pb, + class btIDebugDraw* debugDraw + ); + +}; + +#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp new file mode 100644 index 00000000000..fafceafa5ed --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -0,0 +1,246 @@ +/* +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 "btPersistentManifold.h" +#include "LinearMath/btTransform.h" +#include <assert.h> + +float gContactBreakingTreshold = 0.02f; +ContactDestroyedCallback gContactDestroyedCallback = 0; + + + +btPersistentManifold::btPersistentManifold() +:m_body0(0), +m_body1(0), +m_cachedPoints (0), +m_index1(0) +{ +} + + +void btPersistentManifold::clearManifold() +{ + int i; + for (i=0;i<m_cachedPoints;i++) + { + clearUserCache(m_pointCache[i]); + } + m_cachedPoints = 0; +} + +#ifdef DEBUG_PERSISTENCY +#include <stdio.h> +void btPersistentManifold::DebugPersistency() +{ + int i; + printf("DebugPersistency : numPoints %d\n",m_cachedPoints); + for (i=0;i<m_cachedPoints;i++) + { + printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData); + } +} +#endif //DEBUG_PERSISTENCY + +void btPersistentManifold::clearUserCache(btManifoldPoint& pt) +{ + + void* oldPtr = pt.m_userPersistentData; + if (oldPtr) + { +#ifdef DEBUG_PERSISTENCY + int i; + int occurance = 0; + for (i=0;i<m_cachedPoints;i++) + { + if (m_pointCache[i].m_userPersistentData == oldPtr) + { + occurance++; + if (occurance>1) + printf("error in clearUserCache\n"); + } + } + assert(occurance<=0); +#endif //DEBUG_PERSISTENCY + + if (pt.m_userPersistentData && gContactDestroyedCallback) + { + (*gContactDestroyedCallback)(pt.m_userPersistentData); + pt.m_userPersistentData = 0; + } + +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif + } + + +} + + +int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) +{ + + //calculate 4 possible cases areas, and take biggest area + //also need to keep 'deepest' + + int maxPenetrationIndex = -1; +#define KEEP_DEEPEST_POINT 1 +#ifdef KEEP_DEEPEST_POINT + float maxPenetration = pt.getDistance(); + for (int i=0;i<4;i++) + { + if (m_pointCache[i].getDistance() < maxPenetration) + { + maxPenetrationIndex = i; + maxPenetration = m_pointCache[i].getDistance(); + } + } +#endif //KEEP_DEEPEST_POINT + + btScalar res0(0.f),res1(0.f),res2(0.f),res3(0.f); + if (maxPenetrationIndex != 0) + { + btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; + btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 cross = a0.cross(b0); + res0 = cross.length2(); + } + if (maxPenetrationIndex != 1) + { + btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; + btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + btVector3 cross = a1.cross(b1); + res1 = cross.length2(); + } + + if (maxPenetrationIndex != 2) + { + btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; + btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 cross = a2.cross(b2); + res2 = cross.length2(); + } + + if (maxPenetrationIndex != 3) + { + btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; + btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + btVector3 cross = a3.cross(b3); + res3 = cross.length2(); + } + + btVector4 maxvec(res0,res1,res2,res3); + int biggestarea = maxvec.closestAxis4(); + return biggestarea; +} + + +int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const +{ + btScalar shortestDist = getContactBreakingTreshold() * getContactBreakingTreshold(); + int size = getNumContacts(); + int nearestPoint = -1; + for( int i = 0; i < size; i++ ) + { + const btManifoldPoint &mp = m_pointCache[i]; + + btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + const btScalar distToManiPoint = diffA.dot(diffA); + if( distToManiPoint < shortestDist ) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; +} + +void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint) +{ + assert(validContactDistance(newPoint)); + + int insertIndex = getNumContacts(); + if (insertIndex == MANIFOLD_CACHE_SIZE) + { +#if MANIFOLD_CACHE_SIZE >= 4 + //sort cache so best points come first, based on area + insertIndex = sortCachedPoints(newPoint); +#else + insertIndex = 0; +#endif + + + } else + { + m_cachedPoints++; + + + } + replaceContactPoint(newPoint,insertIndex); +} + +float btPersistentManifold::getContactBreakingTreshold() const +{ + return gContactBreakingTreshold; +} + +void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB) +{ + int i; + + /// first refresh worldspace positions and distance + for (i=getNumContacts()-1;i>=0;i--) + { + btManifoldPoint &manifoldPoint = m_pointCache[i]; + manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA ); + manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB ); + manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB); + manifoldPoint.m_lifeTime++; + } + + /// then + btScalar distance2d; + btVector3 projectedDifference,projectedPoint; + for (i=getNumContacts()-1;i>=0;i--) + { + + btManifoldPoint &manifoldPoint = m_pointCache[i]; + //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) + if (!validContactDistance(manifoldPoint)) + { + removeContactPoint(i); + } else + { + //contact also becomes invalid when relative movement orthogonal to normal exceeds margin + projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1; + projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint; + distance2d = projectedDifference.dot(projectedDifference); + if (distance2d > getContactBreakingTreshold()*getContactBreakingTreshold() ) + { + removeContactPoint(i); + } + } + } +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif // +} + + + + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h new file mode 100644 index 00000000000..d0cc2577fb0 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -0,0 +1,140 @@ +/* +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 PERSISTENT_MANIFOLD_H +#define PERSISTENT_MANIFOLD_H + + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" +#include "btManifoldPoint.h" + +struct btCollisionResult; + +///contact breaking and merging treshold +extern float gContactBreakingTreshold; + +typedef bool (*ContactDestroyedCallback)(void* userPersistentData); +extern ContactDestroyedCallback gContactDestroyedCallback; + + + + +#define MANIFOLD_CACHE_SIZE 4 + +///btPersistentManifold maintains contact points, and reduces them to 4. +///It does contact filtering/contact reduction. +class btPersistentManifold +{ + + btManifoldPoint m_pointCache[MANIFOLD_CACHE_SIZE]; + + /// this two body pointers can point to the physics rigidbody class. + /// void* will allow any rigidbody class + void* m_body0; + void* m_body1; + int m_cachedPoints; + + + /// sort cached points so most isolated points come first + int sortCachedPoints(const btManifoldPoint& pt); + + int findContactPoint(const btManifoldPoint* unUsed, int numUnused,const btManifoldPoint& pt); + +public: + + int m_index1; + + btPersistentManifold(); + + btPersistentManifold(void* body0,void* body1) + : m_body0(body0),m_body1(body1),m_cachedPoints(0) + { + } + + inline void* getBody0() { return m_body0;} + inline void* getBody1() { return m_body1;} + + inline const void* getBody0() const { return m_body0;} + inline const void* getBody1() const { return m_body1;} + + void setBodies(void* body0,void* body1) + { + m_body0 = body0; + m_body1 = body1; + } + + void clearUserCache(btManifoldPoint& pt); + +#ifdef DEBUG_PERSISTENCY + void DebugPersistency(); +#endif // + + inline int getNumContacts() const { return m_cachedPoints;} + + inline const btManifoldPoint& getContactPoint(int index) const + { + ASSERT(index < m_cachedPoints); + return m_pointCache[index]; + } + + inline btManifoldPoint& getContactPoint(int index) + { + ASSERT(index < m_cachedPoints); + return m_pointCache[index]; + } + + /// todo: get this margin from the current physics / collision environment + float getContactBreakingTreshold() const; + + int getCacheEntry(const btManifoldPoint& newPoint) const; + + void AddManifoldPoint( const btManifoldPoint& newPoint); + + void removeContactPoint (int index) + { + clearUserCache(m_pointCache[index]); + + int lastUsedIndex = getNumContacts() - 1; + m_pointCache[index] = m_pointCache[lastUsedIndex]; + //get rid of duplicated userPersistentData pointer + m_pointCache[lastUsedIndex].m_userPersistentData = 0; + m_cachedPoints--; + } + void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) + { + assert(validContactDistance(newPoint)); + + clearUserCache(m_pointCache[insertIndex]); + + m_pointCache[insertIndex] = newPoint; + } + + bool validContactDistance(const btManifoldPoint& pt) const + { + return pt.m_distance1 <= getContactBreakingTreshold(); + } + /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin + void refreshContactPoints( const btTransform& trA,const btTransform& trB); + + void clearManifold(); + + + +}; + + + +#endif //PERSISTENT_MANIFOLD_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h new file mode 100644 index 00000000000..a51e2d5e13b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -0,0 +1,57 @@ +/* +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 POINT_COLLECTOR_H +#define POINT_COLLECTOR_H + +#include "btDiscreteCollisionDetectorInterface.h" + + + +struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result +{ + + + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_distance;//negative means penetration + + bool m_hasResult; + + btPointCollector () + : m_distance(1e30f),m_hasResult(false) + { + } + + virtual void setShapeIdentifiers(int partId0,int index0, int partId1,int index1) + { + //?? + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,float depth) + { + if (depth< m_distance) + { + m_hasResult = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + //negative means penetration + m_distance = depth; + } + } +}; + +#endif //POINT_COLLECTOR_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp new file mode 100644 index 00000000000..88ee005792c --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -0,0 +1,101 @@ +/* +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 "btRaycastCallback.h" + +btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to) + : + m_from(from), + m_to(to), + m_hitFraction(1.f) +{ + +} + + + +void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) +{ + + + const btVector3 &vert0=triangle[0]; + const btVector3 &vert1=triangle[1]; + const btVector3 &vert2=triangle[2]; + + btVector3 v10; v10 = vert1 - vert0 ; + btVector3 v20; v20 = vert2 - vert0 ; + + btVector3 triangleNormal; triangleNormal = v10.cross( v20 ); + + const float dist = vert0.dot(triangleNormal); + float dist_a = triangleNormal.dot(m_from) ; + dist_a-= dist; + float dist_b = triangleNormal.dot(m_to); + dist_b -= dist; + + if ( dist_a * dist_b >= 0.0f) + { + return ; // same sign + } + + const float proj_length=dist_a-dist_b; + const float distance = (dist_a)/(proj_length); + // Now we have the intersection point on the plane, we'll see if it's inside the triangle + // Add an epsilon as a tolerance for the raycast, + // in case the ray hits exacly on the edge of the triangle. + // It must be scaled for the triangle size. + + if(distance < m_hitFraction) + { + + + float edge_tolerance =triangleNormal.length2(); + edge_tolerance *= -0.0001f; + btVector3 point; point.setInterpolate3( m_from, m_to, distance); + { + btVector3 v0p; v0p = vert0 - point; + btVector3 v1p; v1p = vert1 - point; + btVector3 cp0; cp0 = v0p.cross( v1p ); + + if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) + { + + + btVector3 v2p; v2p = vert2 - point; + btVector3 cp1; + cp1 = v1p.cross( v2p); + if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) + { + btVector3 cp2; + cp2 = v2p.cross(v0p); + + if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) + { + + if ( dist_a > 0 ) + { + m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex); + } + else + { + m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex); + } + } + } + } + } + } +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h new file mode 100644 index 00000000000..fbb51d30522 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -0,0 +1,42 @@ +/* +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 RAYCAST_TRI_CALLBACK_H +#define RAYCAST_TRI_CALLBACK_H + +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +struct btBroadphaseProxy; + + +class btTriangleRaycastCallback: public btTriangleCallback +{ +public: + + //input + btVector3 m_from; + btVector3 m_to; + + float m_hitFraction; + + btTriangleRaycastCallback(const btVector3& from,const btVector3& to); + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); + + virtual float reportHit(const btVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; + +}; + +#endif //RAYCAST_TRI_CALLBACK_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h new file mode 100644 index 00000000000..cf65f46505b --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -0,0 +1,64 @@ +/* +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 SIMPLEX_SOLVER_INTERFACE_H +#define SIMPLEX_SOLVER_INTERFACE_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btPoint3.h" + +#define NO_VIRTUAL_INTERFACE 1 +#ifdef NO_VIRTUAL_INTERFACE +#include "btVoronoiSimplexSolver.h" +#define btSimplexSolverInterface btVoronoiSimplexSolver +#else + +/// btSimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices +/// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on +/// voronoi regions or barycentric coordinates +class btSimplexSolverInterface +{ + public: + virtual ~btSimplexSolverInterface() {}; + + virtual void reset() = 0; + + virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) = 0; + + virtual bool closest(btVector3& v) = 0; + + virtual btScalar maxVertex() = 0; + + virtual bool fullSimplex() const = 0; + + virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const = 0; + + virtual bool inSimplex(const btVector3& w) = 0; + + virtual void backup_closest(btVector3& v) = 0; + + virtual bool emptySimplex() const = 0; + + virtual void compute_points(btPoint3& p1, btPoint3& p2) = 0; + + virtual int numVertices() const =0; + + +}; +#endif +#endif //SIMPLEX_SOLVER_INTERFACE_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp new file mode 100644 index 00000000000..dc995ca1f72 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -0,0 +1,133 @@ +/* +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 "btSubSimplexConvexCast.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" + + +btSubsimplexConvexCast::btSubsimplexConvexCast (btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) +:m_simplexSolver(simplexSolver), +m_convexA(convexA),m_convexB(convexB) +{ +} + +///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 +#define MAX_ITERATIONS 32 + +bool btSubsimplexConvexCast::calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result) +{ + + btMinkowskiSumShape combi(m_convexA,m_convexB); + btMinkowskiSumShape* convex = &combi; + + btTransform rayFromLocalA; + btTransform rayToLocalA; + + rayFromLocalA = fromA.inverse()* fromB; + rayToLocalA = toA.inverse()* toB; + + + m_simplexSolver->reset(); + + convex->setTransformB(btTransform(rayFromLocalA.getBasis())); + + //float radius = 0.01f; + + btScalar lambda = 0.f; + //todo: need to verify this: + //because of minkowski difference, we need the inverse direction + + btVector3 s = -rayFromLocalA.getOrigin(); + btVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin()); + btVector3 x = s; + btVector3 v; + btVector3 arbitraryPoint = convex->localGetSupportingVertex(r); + + v = x - arbitraryPoint; + + int maxIter = MAX_ITERATIONS; + + btVector3 n; + n.setValue(0.f,0.f,0.f); + bool hasResult = false; + btVector3 c; + + float lastLambda = lambda; + + + float dist2 = v.length2(); + float epsilon = 0.0001f; + + btVector3 w,p; + float VdotR; + + while ( (dist2 > epsilon) && maxIter--) + { + p = convex->localGetSupportingVertex( v); + w = x - p; + + float VdotW = v.dot(w); + + if ( VdotW > 0.f) + { + VdotR = v.dot(r); + + if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON)) + return false; + else + { + lambda = lambda - VdotW / VdotR; + x = s + lambda * r; + m_simplexSolver->reset(); + //check next line + w = x-p; + lastLambda = lambda; + n = v; + hasResult = true; + } + } + m_simplexSolver->addVertex( w, x , p); + 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 = 0.f; + } + } + + //int numiter = MAX_ITERATIONS - maxIter; +// printf("number of iterations: %d", numiter); + result.m_fraction = lambda; + result.m_normal = n; + + return true; +} + + + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h new file mode 100644 index 00000000000..a2a3193b090 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -0,0 +1,50 @@ +/* +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 SUBSIMPLEX_CONVEX_CAST_H +#define SUBSIMPLEX_CONVEX_CAST_H + +#include "btConvexCast.h" +#include "btSimplexSolverInterface.h" +class btConvexShape; + +/// 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 btSubsimplexConvexCast : public btConvexCast +{ + btSimplexSolverInterface* m_simplexSolver; + btConvexShape* m_convexA; + btConvexShape* m_convexB; + +public: + + btSubsimplexConvexCast (btConvexShape* shapeA,btConvexShape* shapeB,btSimplexSolverInterface* 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. + virtual bool calcTimeOfImpact( + const btTransform& fromA, + const btTransform& toA, + const btTransform& fromB, + const btTransform& toB, + CastResult& result); + +}; + +#endif //SUBSIMPLEX_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp new file mode 100644 index 00000000000..23d66a3bbc8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp @@ -0,0 +1,598 @@ + +/* +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. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "btVoronoiSimplexSolver.h" +#include <assert.h> +#include <stdio.h> + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define CATCH_DEGENERATE_TETRAHEDRON 1 +void btVoronoiSimplexSolver::removeVertex(int index) +{ + + assert(m_numVertices>0); + m_numVertices--; + m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; + m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; + m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; +} + +void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) +{ + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) + removeVertex(1); + + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) + removeVertex(0); + +} + + + + + +//clear the simplex, remove all the vertices +void btVoronoiSimplexSolver::reset() +{ + m_cachedValidClosest = false; + m_numVertices = 0; + m_needsUpdate = true; + m_lastW = btVector3(1e30f,1e30f,1e30f); + m_cachedBC.reset(); +} + + + + //add a vertex +void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q) +{ + m_lastW = w; + m_needsUpdate = true; + + m_simplexVectorW[m_numVertices] = w; + m_simplexPointsP[m_numVertices] = p; + m_simplexPointsQ[m_numVertices] = q; + + m_numVertices++; +} + +bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() +{ + + if (m_needsUpdate) + { + m_cachedBC.reset(); + + m_needsUpdate = false; + + switch (numVertices()) + { + case 0: + m_cachedValidClosest = false; + break; + case 1: + { + m_cachedP1 = m_simplexPointsP[0]; + m_cachedP2 = m_simplexPointsQ[0]; + m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedBC.reset(); + m_cachedBC.setBarycentricCoordinates(1.f,0.f,0.f,0.f); + m_cachedValidClosest = m_cachedBC.isValid(); + break; + }; + case 2: + { + //closest point origin from line segment + const btVector3& from = m_simplexVectorW[0]; + const btVector3& to = m_simplexVectorW[1]; + btVector3 nearest; + + btVector3 p (0.f,0.f,0.f); + btVector3 diff = p - from; + btVector3 v = to - from; + float t = v.dot(diff); + + if (t > 0) { + float dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } else { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; + } + } else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1-t,t); + nearest = from + t*v; + + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; + + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; + } + case 3: + { + //closest point origin from triangle + btVector3 p (0.f,0.f,0.f); + + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; + + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; + } + case 4: + { + + + btVector3 p (0.f,0.f,0.f); + + const btVector3& a = m_simplexVectorW[0]; + const btVector3& b = m_simplexVectorW[1]; + const btVector3& c = m_simplexVectorW[2]; + const btVector3& d = m_simplexVectorW[3]; + + bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + + if (hasSeperation) + { + + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + reduceVertices (m_cachedBC.m_usedVertices); + } else + { +// printf("sub distance got penetration\n"); + + if (m_cachedBC.m_degenerate) + { + m_cachedValidClosest = false; + } else + { + m_cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + m_cachedV.setValue(0.f,0.f,0.f); + } + break; + } + + m_cachedValidClosest = m_cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + m_cachedValidClosest = false; + } + }; + } + + return m_cachedValidClosest; + +} + +//return/calculate the closest vertex +bool btVoronoiSimplexSolver::closest(btVector3& v) +{ + bool succes = updateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +btScalar btVoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + btScalar maxV = 0.f; + for (i=0;i<numverts;i++) + { + btScalar curLen2 = m_simplexVectorW[i].length2(); + if (maxV < curLen2) + maxV = curLen2; + } + return maxV; +} + + + + //return the current simplex +int btVoronoiSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const +{ + int i; + for (i=0;i<numVertices();i++) + { + yBuf[i] = m_simplexVectorW[i]; + pBuf[i] = m_simplexPointsP[i]; + qBuf[i] = m_simplexPointsQ[i]; + } + return numVertices(); +} + + + + +bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) +{ + bool found = false; + int i, numverts = numVertices(); + //btScalar maxV = 0.f; + + //w is in the current (reduced) simplex + for (i=0;i<numverts;i++) + { + if (m_simplexVectorW[i] == w) + found = true; + } + + //check in case lastW is already removed + if (w == m_lastW) + return true; + + return found; +} + +void btVoronoiSimplexSolver::backup_closest(btVector3& v) +{ + v = m_cachedV; +} + + +bool btVoronoiSimplexSolver::emptySimplex() const +{ + return (numVertices() == 0); + +} + +void btVoronoiSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2) +{ + updateClosestVectorAndPoints(); + p1 = m_cachedP1; + p2 = m_cachedP2; + +} + + + + +bool btVoronoiSimplexSolver::closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result) +{ + result.m_usedVertices.reset(); + + // Check if P in vertex region outside A + btVector3 ab = b - a; + btVector3 ac = c - a; + btVector3 ap = p - a; + float d1 = ab.dot(ap); + float d2 = ac.dot(ap); + if (d1 <= 0.0f && d2 <= 0.0f) + { + result.m_closestPointOnSimplex = a; + result.m_usedVertices.usedVertexA = true; + result.setBarycentricCoordinates(1,0,0); + return true;// a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + btVector3 bp = p - b; + float d3 = ab.dot(bp); + float d4 = ac.dot(bp); + if (d3 >= 0.0f && d4 <= d3) + { + result.m_closestPointOnSimplex = b; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(0,1,0); + + return true; // b; // barycentric coordinates (0,1,0) + } + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) { + float v = d1 / (d1 - d3); + result.m_closestPointOnSimplex = a + v * ab; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(1-v,v,0); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + btVector3 cp = p - c; + float d5 = ab.dot(cp); + float d6 = ac.dot(cp); + if (d6 >= 0.0f && d5 <= d6) + { + result.m_closestPointOnSimplex = c; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,0,1); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) { + float w = d2 / (d2 - d6); + result.m_closestPointOnSimplex = a + w * ac; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-w,0,w); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) { + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + result.m_closestPointOnSimplex = b + w * (c - b); + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,1-w,w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1.0f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + + result.m_closestPointOnSimplex = a + ab * v + ac * w; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-v-w,v,w); + + return true; +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w + +} + + + + + +/// Test if point p and d lie on opposite sides of plane through abc +int btVoronoiSimplexSolver::pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d) +{ + btVector3 normal = (b-a).cross(c-a); + + float signp = (p - a).dot(normal); // [AP AB AC] + float signd = (d - a).dot( normal); // [AD AB AC] + +#ifdef CATCH_DEGENERATE_TETRAHEDRON + if (signd * signd < (1e-4f * 1e-4f)) + { +// printf("affine dependent/degenerate\n");// + return -1; + } +#endif + // Points on opposite sides if expression signs are opposite + return signp * signd < 0.f; +} + + +bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult) +{ + btSubSimplexClosestResult tempResult; + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.m_closestPointOnSimplex = p; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexB = true; + finalResult.m_usedVertices.usedVertexC = true; + finalResult.m_usedVertices.usedVertexD = true; + + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + + float bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + closestPtPointTriangle(p, a, b, c,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + + float sqDist = (q - p).dot( q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + //convert result bitmask! + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD) + { + closestPtPointTriangle(p, a, c, d,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + float sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB) + { + closestPtPointTriangle(p, a, d, b,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + float sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC) + { + closestPtPointTriangle(p, b, d, c,tempResult); + btPoint3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + float sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + + finalResult.setBarycentricCoordinates( + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + + //help! we ended up full ! + + if (finalResult.m_usedVertices.usedVertexA && + finalResult.m_usedVertices.usedVertexB && + finalResult.m_usedVertices.usedVertexC && + finalResult.m_usedVertices.usedVertexD) + { + return true; + } + + return true; +} + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h new file mode 100644 index 00000000000..8b743996915 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -0,0 +1,157 @@ +/* +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 btVoronoiSimplexSolver_H +#define btVoronoiSimplexSolver_H + +#include "btSimplexSolverInterface.h" + + + +#define VORONOI_SIMPLEX_MAX_VERTS 5 + +struct btUsageBitfield{ + btUsageBitfield() + { + reset(); + } + + void reset() + { + usedVertexA = false; + usedVertexB = false; + usedVertexC = false; + usedVertexD = false; + } + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; +}; + + +struct btSubSimplexClosestResult +{ + btPoint3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + btUsageBitfield m_usedVertices; + float m_barycentricCoords[4]; + bool m_degenerate; + + void reset() + { + m_degenerate = false; + setBarycentricCoordinates(); + m_usedVertices.reset(); + } + bool isValid() + { + bool valid = (m_barycentricCoords[0] >= 0.f) && + (m_barycentricCoords[1] >= 0.f) && + (m_barycentricCoords[2] >= 0.f) && + (m_barycentricCoords[3] >= 0.f); + + + return valid; + } + void setBarycentricCoordinates(float a=0.f,float b=0.f,float c=0.f,float d=0.f) + { + m_barycentricCoords[0] = a; + m_barycentricCoords[1] = b; + m_barycentricCoords[2] = c; + m_barycentricCoords[3] = d; + } + +}; + +/// btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. +/// Can be used with GJK, as an alternative to Johnson distance algorithm. +#ifdef NO_VIRTUAL_INTERFACE +class btVoronoiSimplexSolver +#else +class btVoronoiSimplexSolver : public btSimplexSolverInterface +#endif +{ +public: + + int m_numVertices; + + btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + btPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + btPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + + + + btPoint3 m_cachedP1; + btPoint3 m_cachedP2; + btVector3 m_cachedV; + btVector3 m_lastW; + bool m_cachedValidClosest; + + btSubSimplexClosestResult m_cachedBC; + + bool m_needsUpdate; + + void removeVertex(int index); + void reduceVertices (const btUsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); + + bool closestPtPointTetrahedron(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d, btSubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c, const btPoint3& d); + bool closestPtPointTriangle(const btPoint3& p, const btPoint3& a, const btPoint3& b, const btPoint3& c,btSubSimplexClosestResult& result); + +public: + + void reset(); + + void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q); + + + bool closest(btVector3& v); + + btScalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const; + + bool inSimplex(const btVector3& w); + + void backup_closest(btVector3& v) ; + + bool emptySimplex() const ; + + void compute_points(btPoint3& p1, btPoint3& p2) ; + + int numVertices() const + { + return m_numVertices; + } + + +}; + +#endif //VoronoiSimplexSolver diff --git a/extern/bullet2/src/BulletDynamics/CMakeLists.txt b/extern/bullet2/src/BulletDynamics/CMakeLists.txt new file mode 100644 index 00000000000..79e07b7f77b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/CMakeLists.txt @@ -0,0 +1,19 @@ +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src } +) + +ADD_LIBRARY(LibBulletDynamics + + ConstraintSolver/btContactConstraint.cpp + ConstraintSolver/btGeneric6DofConstraint.cpp + ConstraintSolver/btHingeConstraint.cpp + ConstraintSolver/btPoint2PointConstraint.cpp + ConstraintSolver/btSequentialImpulseConstraintSolver.cpp + ConstraintSolver/btSolve2LinearConstraint.cpp + ConstraintSolver/btTypedConstraint.cpp + Dynamics/btDiscreteDynamicsWorld.cpp + Dynamics/btSimpleDynamicsWorld.cpp + Dynamics/btRigidBody.cpp + Vehicle/btRaycastVehicle.cpp + Vehicle/btWheelInfo.cpp +) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h new file mode 100644 index 00000000000..e073797f989 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -0,0 +1,41 @@ +/* +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 CONSTRAINT_SOLVER_H +#define CONSTRAINT_SOLVER_H + +class btPersistentManifold; +class btRigidBody; + +struct btContactSolverInfo; +struct btBroadphaseProxy; +class btIDebugDraw; + +/// btConstraintSolver provides solver interface +class btConstraintSolver +{ + +public: + + virtual ~btConstraintSolver() {} + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + +}; + + + + +#endif //CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp new file mode 100644 index 00000000000..9019035f586 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -0,0 +1,234 @@ +/* +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 "btContactConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btContactSolverInfo.h" +#include "LinearMath/btMinMax.h" +#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" + +#define ASSERT2 assert + +//some values to find stable tresholds + +float useGlobalSettingContacts = false;//true; +btScalar contactDamping = 0.2f; +btScalar contactTau = .02f;//0.02f;//*0.02f; + + + + + + + +//bilateral constraint between two dynamic objects +void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep) +{ + float normalLenSqr = normal.length2(); + ASSERT2(fabs(normalLenSqr) < 1.1f); + if (normalLenSqr > 1.1f) + { + impulse = 0.f; + return; + } + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + + btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), + body2.getInvInertiaDiagLocal(),body2.getInvMass()); + + btScalar jacDiagAB = jac.getDiagonal(); + btScalar jacDiagABInv = 1.f / jacDiagAB; + + btScalar rel_vel = jac.getRelativeVelocity( + body1.getLinearVelocity(), + body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), + body2.getLinearVelocity(), + body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); + float a; + a=jacDiagABInv; + + + rel_vel = normal.dot(vel); + + +#ifdef ONLY_USE_LINEAR_MASS + btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + impulse = - contactDamping * rel_vel * massTerm; +#else + btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; +#endif +} + + + + +//velocity + friction +//response between two dynamic objects with friction +float resolveSingleCollision( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + + ) +{ + + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); + const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + + +// printf("distance=%f\n",distance); + + const btVector3& normal = contactPoint.m_normalWorldOnB; + + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + + btScalar Kfps = 1.f / solverInfo.m_timeStep ; + + float damping = solverInfo.m_damping ; + float Kerp = solverInfo.m_erp; + + if (useGlobalSettingContacts) + { + damping = contactDamping; + Kerp = contactTau; + } + + float Kcor = Kerp *Kfps; + + //printf("dist=%f\n",distance); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + btScalar distance = cpd->m_penetration;//contactPoint.getDistance(); + + + //distance = 0.f; + btScalar positionalError = Kcor *-distance; + //jacDiagABInv; + btScalar velocityError = cpd->m_restitution - rel_vel;// * damping; + + + btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; + + btScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; + + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd->m_appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum; + + normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse; + + body1.applyImpulse(normal*(normalImpulse), rel_pos1); + body2.applyImpulse(-normal*(normalImpulse), rel_pos2); + + return normalImpulse; +} + + +float resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + + ) +{ + + const btVector3& pos1 = contactPoint.getPositionWorldOnA(); + const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + float combinedFriction = cpd->m_friction; + + btScalar limit = cpd->m_appliedImpulse * combinedFriction; + //if (contactPoint.m_appliedImpulse>0.f) + //friction + { + //apply friction in the 2 tangential directions + + { + // 1st tangent + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); + + // calculate j that moves us to zero relative velocity + btScalar j = -vrel * cpd->m_jacDiagABInvTangent0; + float total = cpd->m_accumulatedTangentImpulse0 + j; + GEN_set_min(total, limit); + GEN_set_max(total, -limit); + j = total - cpd->m_accumulatedTangentImpulse0; + cpd->m_accumulatedTangentImpulse0 = total; + body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1); + body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2); + } + + + { + // 2nd tangent + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); + + // calculate j that moves us to zero relative velocity + btScalar j = -vrel * cpd->m_jacDiagABInvTangent1; + float total = cpd->m_accumulatedTangentImpulse1 + j; + GEN_set_min(total, limit); + GEN_set_max(total, -limit); + j = total - cpd->m_accumulatedTangentImpulse1; + cpd->m_accumulatedTangentImpulse1 = total; + body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1); + body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2); + } + } + return cpd->m_appliedImpulse; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h new file mode 100644 index 00000000000..42ded30ae04 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -0,0 +1,104 @@ +/* +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 CONTACT_CONSTRAINT_H +#define CONTACT_CONSTRAINT_H + +//todo: make into a proper class working with the iterative constraint solver + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btScalar.h" +struct btContactSolverInfo; +class btManifoldPoint; + +enum { + DEFAULT_CONTACT_SOLVER_TYPE=0, + CONTACT_SOLVER_TYPE1, + CONTACT_SOLVER_TYPE2, + USER_CONTACT_SOLVER_TYPE1, + MAX_CONTACT_SOLVER_TYPES +}; + + +typedef float (*ContactSolverFunc)(btRigidBody& body1, + btRigidBody& body2, + class btManifoldPoint& contactPoint, + const btContactSolverInfo& info); + +///stores some extra information to each contact point. It is not in the contact point, because that want to keep the collision detection independent from the constraint solver. +struct btConstraintPersistentData +{ + inline btConstraintPersistentData() + :m_appliedImpulse(0.f), + m_prevAppliedImpulse(0.f), + m_accumulatedTangentImpulse0(0.f), + m_accumulatedTangentImpulse1(0.f), + m_jacDiagABInv(0.f), + m_persistentLifeTime(0), + m_restitution(0.f), + m_friction(0.f), + m_penetration(0.f), + m_contactSolverFunc(0), + m_frictionSolverFunc(0) + { + } + + + /// total applied impulse during most recent frame + float m_appliedImpulse; + float m_prevAppliedImpulse; + float m_accumulatedTangentImpulse0; + float m_accumulatedTangentImpulse1; + + float m_jacDiagABInv; + float m_jacDiagABInvTangent0; + float m_jacDiagABInvTangent1; + int m_persistentLifeTime; + float m_restitution; + float m_friction; + float m_penetration; + btVector3 m_frictionWorldTangential0; + btVector3 m_frictionWorldTangential1; + + ContactSolverFunc m_contactSolverFunc; + ContactSolverFunc m_frictionSolverFunc; + +}; + +///bilateral constraint between two dynamic objects +///positive distance = separation, negative distance = penetration +void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, + btRigidBody& body2, const btVector3& pos2, + btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep); + + +///contact constraint resolution: +///calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint +///positive distance = separation, negative distance = penetration +float resolveSingleCollision( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& info); + +float resolveSingleFriction( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + +#endif //CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h new file mode 100644 index 00000000000..ed1ba6ac1ba --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -0,0 +1,47 @@ +/* +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 CONTACT_SOLVER_INFO +#define CONTACT_SOLVER_INFO + + +struct btContactSolverInfo +{ + + inline btContactSolverInfo() + { + m_tau = 0.6f; + m_damping = 1.0f; + m_friction = 0.3f; + m_restitution = 0.f; + m_maxErrorReduction = 20.f; + m_numIterations = 10; + m_erp = 0.4f; + m_sor = 1.3f; + } + + float m_tau; + float m_damping; + float m_friction; + float m_timeStep; + float m_restitution; + int m_numIterations; + float m_maxErrorReduction; + float m_sor; + float m_erp; + +}; + +#endif //CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp new file mode 100644 index 00000000000..a8ab1bbad3a --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -0,0 +1,250 @@ +/* +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 "btGeneric6DofConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + +static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f }; +static const int kAxisA[] = { 1, 0, 0 }; +static const int kAxisB[] = { 2, 2, 1 }; + +btGeneric6DofConstraint::btGeneric6DofConstraint() +{ +} + +btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) +: btTypedConstraint(rbA, rbB) +, m_frameInA(frameInA) +, m_frameInB(frameInB) +{ + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //so start all locked + for (int i=0; i<6;++i) + { + m_lowerLimit[i] = 0.0f; + m_upperLimit[i] = 0.0f; + m_accumulatedImpulse[i] = 0.0f; + } + +} + + +void btGeneric6DofConstraint::buildJacobian() +{ + btVector3 normal(0,0,0); + + const btVector3& pivotInA = m_frameInA.getOrigin(); + const btVector3& pivotInB = m_frameInB.getOrigin(); + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + int i; + //linear part + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + normal[i] = 1; + + // Create linear atom + new (&m_jacLinear[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + + // Apply accumulated impulse + btVector3 impulse_vector = m_accumulatedImpulse[i] * normal; + + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular part + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe + btVector3 axis = kSign[i] * axisA.cross(axisB); + + // Create angular atom + new (&m_jacAng[i]) btJacobianEntry(axis, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + // Apply accumulated impulse + btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void btGeneric6DofConstraint::solveConstraint(btScalar timeStep) +{ + btScalar tau = 0.1f; + btScalar damping = 1.0f; + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 normal(0,0,0); + int i; + + // linear + for (i=0;i<3;i++) + { + if (isLimited(i)) + { + btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal(); + + //velocity error (first order error) + btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + btScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i] += impulse; + + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, rel_pos1); + m_rbB.applyImpulse(-impulse_vector, rel_pos2); + + normal[i] = 0; + } + } + + // angular + for (i=0;i<3;i++) + { + if (isLimited(i+3)) + { + btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal(); + + //velocity error (first order error) + btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] ); + btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] ); + + btScalar rel_pos = kSign[i] * axisA.dot(axisB); + + //impulse + btScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv; + m_accumulatedImpulse[i + 3] += impulse; + + // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above) + btVector3 axis = kSign[i] * axisA.cross(axisB); + btVector3 impulse_vector = axis * impulse; + + m_rbA.applyTorqueImpulse( impulse_vector); + m_rbB.applyTorqueImpulse(-impulse_vector); + } + } +} + +void btGeneric6DofConstraint::updateRHS(btScalar timeStep) +{ + +} + +btScalar btGeneric6DofConstraint::computeAngle(int axis) const + { + btScalar angle; + + switch (axis) + { + case 0: + { + btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1); + btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + + btScalar s = v1.dot(w2); + btScalar c = v1.dot(v2); + + angle = btAtan2( s, c ); + } + break; + + case 1: + { + btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2); + btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2); + btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + + btScalar s = w1.dot(u2); + btScalar c = w1.dot(w2); + + angle = btAtan2( s, c ); + } + break; + + case 2: + { + btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0); + btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0); + btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1); + + btScalar s = u1.dot(v2); + btScalar c = u1.dot(u2); + + angle = btAtan2( s, c ); + } + break; + default: assert ( 0 ) ; break ; + } + + return angle; + } + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h new file mode 100644 index 00000000000..329048b5737 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -0,0 +1,114 @@ +/* +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 GENERIC_6DOF_CONSTRAINT_H +#define GENERIC_6DOF_CONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + + +/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' +/// Work in progress (is still a Hinge actually) +class btGeneric6DofConstraint : public btTypedConstraint +{ + btJacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints + btJacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints + + btTransform m_frameInA; // the constraint space w.r.t body A + btTransform m_frameInB; // the constraint space w.r.t body B + + btScalar m_lowerLimit[6]; // the constraint lower limits + btScalar m_upperLimit[6]; // the constraint upper limits + + btScalar m_accumulatedImpulse[6]; + + +public: + btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ); + + btGeneric6DofConstraint(); + + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + btScalar computeAngle(int axis) const; + + void setLinearLowerLimit(const btVector3& linearLower) + { + m_lowerLimit[0] = linearLower.getX(); + m_lowerLimit[1] = linearLower.getY(); + m_lowerLimit[2] = linearLower.getZ(); + } + + void setLinearUpperLimit(const btVector3& linearUpper) + { + m_upperLimit[0] = linearUpper.getX(); + m_upperLimit[1] = linearUpper.getY(); + m_upperLimit[2] = linearUpper.getZ(); + } + + void setAngularLowerLimit(const btVector3& angularLower) + { + m_lowerLimit[3] = angularLower.getX(); + m_lowerLimit[4] = angularLower.getY(); + m_lowerLimit[5] = angularLower.getZ(); + } + + void setAngularUpperLimit(const btVector3& angularUpper) + { + m_upperLimit[3] = angularUpper.getX(); + m_upperLimit[4] = angularUpper.getY(); + m_upperLimit[5] = angularUpper.getZ(); + } + + //first 3 are linear, next 3 are angular + void SetLimit(int axis, btScalar lo, btScalar hi) + { + m_lowerLimit[axis] = lo; + m_upperLimit[axis] = hi; + } + + //free means upper < lower, + //locked means upper == lower + //limited means upper > lower + //limitIndex: first 3 are linear, next 3 are angular + bool isLimited(int limitIndex) + { + return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); + } + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + +}; + +#endif //GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp new file mode 100644 index 00000000000..b507e4c7bb8 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -0,0 +1,275 @@ +/* +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 "btHingeConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" + + +btHingeConstraint::btHingeConstraint() +{ +} + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, + btVector3& axisInA,btVector3& axisInB) +:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_axisInA(axisInA), +m_axisInB(-axisInB), +m_angularOnly(false) +{ + +} + + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) +:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), +m_axisInA(axisInA), +//fixed axis in worldspace +m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), +m_angularOnly(false) +{ + +} + +void btHingeConstraint::buildJacobian() +{ + m_appliedImpulse = 0.f; + + btVector3 normal(0,0,0); + + if (!m_angularOnly) + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } + } + + //calculate two perpendicular jointAxis, orthogonal to hingeAxis + //these two jointAxis require equal angular velocities for both bodies + + //this is unused for now, it's a todo + btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 jointAxis0; + btVector3 jointAxis1; + btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); + + new (&m_jacAng[0]) btJacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) btJacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + +} + +void btHingeConstraint::solveConstraint(btScalar timeStep) +{ +//#define NEW_IMPLEMENTATION + +#ifdef NEW_IMPLEMENTATION + btScalar tau = 0.3f; + btScalar damping = 1.f; + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + // Dirk: Don't we need to update this after each applied impulse + btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + + if (!m_angularOnly) + { + btVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); + + btScalar impulse = tau*depth/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv; + + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } + } + + ///solve angular part + + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + // constraint axes in world space + btVector3 jointAxis0; + btVector3 jointAxis1; + btPlaneSpace1(axisA,jointAxis0,jointAxis1); + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal(); + btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + float tau1 = tau;//0.f; + + btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0; + btVector3 angular_impulse0 = jointAxis0 * impulse0; + + m_rbA.applyTorqueImpulse( angular_impulse0); + m_rbB.applyTorqueImpulse(-angular_impulse0); + + + + // Dirk: Get new angular velocity since it changed after applying an impulse + angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal(); + btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB);; + + btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1; + btVector3 angular_impulse1 = jointAxis1 * impulse1; + + m_rbA.applyTorqueImpulse( angular_impulse1); + m_rbB.applyTorqueImpulse(-angular_impulse1); + +#else + + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + btVector3 normal(0,0,0); + btScalar tau = 0.3f; + btScalar damping = 1.f; + +//linear part + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } + } + + ///solve angular part + + // get axes in world space + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); + const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + btVector3 angA = angVelA - axisA * axisA.dot(angVelA); + btVector3 angB = angVelB - axisB * axisB.dot(angVelB); + btVector3 velrel = angA-angB; + + //solve angular velocity correction + float relaxation = 1.f; + float len = velrel.length(); + if (len > 0.00001f) + { + btVector3 normal = velrel.normalized(); + float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + velrel *= (1.f/denom) * 0.9; + } + + //solve angular positional correction + btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + btVector3 normal2 = angularError.normalized(); + float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + angularError *= (1.f/denom2) * relaxation; + } + + m_rbA.applyTorqueImpulse(-velrel+angularError); + m_rbB.applyTorqueImpulse(velrel-angularError); + +#endif + +} + +void btHingeConstraint::updateRHS(btScalar timeStep) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h new file mode 100644 index 00000000000..3bade2b091d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -0,0 +1,73 @@ +/* +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 HINGECONSTRAINT_H +#define HINGECONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space +/// axis defines the orientation of the hinge axis +class btHingeConstraint : public btTypedConstraint +{ + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints + + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btVector3 m_axisInA; + btVector3 m_axisInB; + + bool m_angularOnly; + +public: + + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB); + + btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + + btHingeConstraint(); + + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + + + +}; + +#endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h new file mode 100644 index 00000000000..bb2aecfb6d0 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -0,0 +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. +*/ + +#ifndef JACOBIAN_ENTRY_H +#define JACOBIAN_ENTRY_H + +#include "LinearMath/btVector3.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + +//notes: +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components +// which makes the btJacobianEntry memory layout 16 bytes +// if you only are interested in angular part, just feed massInvA and massInvB zero + +/// Jacobian entry is an abstraction that allows to describe constraints +/// it can be used in combination with a constraint solver +/// Can be used to relate the effect of an impulse to the constraint error +class btJacobianEntry +{ +public: + btJacobianEntry() {}; + //constraint between two different rigidbodies + btJacobianEntry( + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& jointAxis, + const btVector3& inertiaInvA, + const btScalar massInvA, + const btVector3& inertiaInvB, + const btScalar massInvB) + :m_linearJointAxis(jointAxis) + { + m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //angular constraint between two different rigidbodies + btJacobianEntry(const btVector3& jointAxis, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + :m_linearJointAxis(btVector3(0.f,0.f,0.f)) + { + m_aJ= world2A*jointAxis; + m_bJ = world2B*-jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //angular constraint between two different rigidbodies + btJacobianEntry(const btVector3& axisInA, + const btVector3& axisInB, + const btVector3& inertiaInvA, + const btVector3& inertiaInvB) + : m_linearJointAxis(btVector3(0.f,0.f,0.f)) + , m_aJ(axisInA) + , m_bJ(-axisInB) + { + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + + ASSERT(m_Adiag > 0.0f); + } + + //constraint on one rigidbody + btJacobianEntry( + const btMatrix3x3& world2A, + const btVector3& rel_pos1,const btVector3& rel_pos2, + const btVector3& jointAxis, + const btVector3& inertiaInvA, + const btScalar massInvA) + :m_linearJointAxis(jointAxis) + { + m_aJ= world2A*(rel_pos1.cross(jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = btVector3(0.f,0.f,0.f); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + + ASSERT(m_Adiag > 0.0f); + } + + btScalar getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + btScalar getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const + { + const btJacobianEntry& jacA = *this; + btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis); + btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ); + return lin + ang; + } + + + + // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) + btScalar getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const + { + const btJacobianEntry& jacA = *this; + btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis; + btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + btVector3 lin0 = massInvA * lin ; + btVector3 lin1 = massInvB * lin; + btVector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB) + { + btVector3 linrel = linvelA - linvelB; + btVector3 angvela = angvelA * m_aJ; + btVector3 angvelb = angvelB * m_bJ; + linrel *= m_linearJointAxis; + angvela += angvelb; + angvela += linrel; + btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + SIMD_EPSILON; + } +//private: + + btVector3 m_linearJointAxis; + btVector3 m_aJ; + btVector3 m_bJ; + btVector3 m_0MinvJt; + btVector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + btScalar m_Adiag; + +}; + +#endif //JACOBIAN_ENTRY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp new file mode 100644 index 00000000000..d15bdaad790 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -0,0 +1,115 @@ +/* +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 "btPoint2PointConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + + + + +btPoint2PointConstraint::btPoint2PointConstraint() +{ +} + +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) +:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +{ + +} + + +btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) +:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +{ + +} + +void btPoint2PointConstraint::buildJacobian() +{ + m_appliedImpulse = 0.f; + + btVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), + m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), + normal, + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + normal[i] = 0; + } + +} + +void btPoint2PointConstraint::solveConstraint(btScalar timeStep) +{ + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + btVector3 normal(0,0,0); + + +// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); +// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + btScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + m_appliedImpulse+=impulse; + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + + normal[i] = 0; + } +} + +void btPoint2PointConstraint::updateRHS(btScalar timeStep) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h new file mode 100644 index 00000000000..8aae8d74ce7 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -0,0 +1,78 @@ +/* +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 POINT2POINTCONSTRAINT_H +#define POINT2POINTCONSTRAINT_H + +#include "LinearMath/btVector3.h" + +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + +struct btConstraintSetting +{ + btConstraintSetting() : + m_tau(0.3f), + m_damping(1.f) + { + } + float m_tau; + float m_damping; +}; + +/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space +class btPoint2PointConstraint : public btTypedConstraint +{ + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + + btVector3 m_pivotInA; + btVector3 m_pivotInB; + + + +public: + + btConstraintSetting m_setting; + + btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); + + btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA); + + btPoint2PointConstraint(); + + virtual void buildJacobian(); + + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + void setPivotA(const btVector3& pivotA) + { + m_pivotInA = pivotA; + } + + void setPivotB(const btVector3& pivotB) + { + m_pivotInB = pivotB; + } + + + +}; + +#endif //POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp new file mode 100644 index 00000000000..e747177a516 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -0,0 +1,361 @@ +/* +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 "btSequentialImpulseConstraintSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "btContactConstraint.h" +#include "btSolve2LinearConstraint.h" +#include "btContactSolverInfo.h" +#include "LinearMath/btIDebugDraw.h" +#include "btJacobianEntry.h" +#include "LinearMath/btMinMax.h" + +#ifdef USE_PROFILE +#include "LinearMath/btQuickprof.h" +#endif //USE_PROFILE + +int totalCpd = 0; + +int gTotalContactPoints = 0; + +bool MyContactDestroyedCallback(void* userPersistentData) +{ + assert (userPersistentData); + btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData; + delete cpd; + totalCpd--; + //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); + return true; +} + + +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +{ + gContactDestroyedCallback = &MyContactDestroyedCallback; + + //initialize default friction/contact funcs + int i,j; + for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++) + for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++) + { + + m_contactDispatch[i][j] = resolveSingleCollision; + m_frictionDispatch[i][j] = resolveSingleFriction; + } +} + +/// btSequentialImpulseConstraintSolver Sequentially applies impulses +float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +{ + + btContactSolverInfo info = infoGlobal; + + int numiter = infoGlobal.m_numIterations; +#ifdef USE_PROFILE + btProfiler::beginBlock("solve"); +#endif //USE_PROFILE + + { + int j; + for (j=0;j<numManifolds;j++) + { + int k=j; + //interleaving the preparation with solving impacts the behaviour a lot, todo: find out why + + prepareConstraints(manifoldPtr[k],info,debugDrawer); + solve(manifoldPtr[k],info,0,debugDrawer); + } + } + + + //should traverse the contacts random order... + int i; + for ( i = 0;i<numiter-1;i++) + { + int j; + for (j=0;j<numManifolds;j++) + { + int k=j; + if (i&1) + k=numManifolds-j-1; + + solve(manifoldPtr[k],info,i,debugDrawer); + } + + } +#ifdef USE_PROFILE + btProfiler::endBlock("solve"); + + btProfiler::beginBlock("solveFriction"); +#endif //USE_PROFILE + + //now solve the friction + for (i = 0;i<numiter-1;i++) + { + int j; + for (j=0;j<numManifolds;j++) + { + int k = j; + if (i&1) + k=numManifolds-j-1; + solveFriction(manifoldPtr[k],info,i,debugDrawer); + } + } +#ifdef USE_PROFILE + btProfiler::endBlock("solveFriction"); +#endif //USE_PROFILE + + return 0.f; +} + + +float penetrationResolveFactor = 0.9f; +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + +void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer) +{ + + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + + //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop + { + manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); + + int numpoints = manifoldPtr->getNumContacts(); + + gTotalContactPoints += numpoints; + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + btManifoldPoint& cp = manifoldPtr->getContactPoint(i); + if (cp.getDistance() <= 0.f) + { + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); + + + //this jacobian entry is re-used for all iterations + btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), + body1->getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), + body1->getInvInertiaDiagLocal(),body1->getInvMass()); + + + btScalar jacDiagAB = jac.getDiagonal(); + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + if (cpd) + { + //might be invalid + cpd->m_persistentLifeTime++; + if (cpd->m_persistentLifeTime != cp.getLifeTime()) + { + //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + new (cpd) btConstraintPersistentData; + cpd->m_persistentLifeTime = cp.getLifeTime(); + + } else + { + //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + + } + } else + { + + cpd = new btConstraintPersistentData(); + totalCpd ++; + //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); + cp.m_userPersistentData = cpd; + cpd->m_persistentLifeTime = cp.getLifeTime(); + //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); + + } + assert(cpd); + + cpd->m_jacDiagABInv = 1.f / jacDiagAB; + + //Dependent on Rigidbody A and B types, fetch the contact/friction response func + //perhaps do a similar thing for friction/restutution combiner funcs... + + cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; + cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; + + btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + float combinedRestitution = cp.m_combinedRestitution; + + cpd->m_penetration = cp.getDistance(); + cpd->m_friction = cp.m_combinedFriction; + cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); + if (cpd->m_restitution <= 0.) //0.f) + { + cpd->m_restitution = 0.0f; + + }; + + //restitution and penetration work in same direction so + //rel_vel + + btScalar penVel = -cpd->m_penetration/info.m_timeStep; + + if (cpd->m_restitution >= penVel) + { + cpd->m_penetration = 0.f; + } + + + float relaxation = info.m_damping; + cpd->m_appliedImpulse *= relaxation; + //for friction + cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse; + + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1); + + +#define NO_FRICTION_WARMSTART 1 + + #ifdef NO_FRICTION_WARMSTART + cpd->m_accumulatedTangentImpulse0 = 0.f; + cpd->m_accumulatedTangentImpulse1 = 0.f; + #endif //NO_FRICTION_WARMSTART + float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0); + float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0); + float denom = relaxation/(denom0+denom1); + cpd->m_jacDiagABInvTangent0 = denom; + + + denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1); + denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1); + denom = relaxation/(denom0+denom1); + cpd->m_jacDiagABInvTangent1 = denom; + + btVector3 totalImpulse = + #ifndef NO_FRICTION_WARMSTART + cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+ + cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+ + #endif //NO_FRICTION_WARMSTART + cp.m_normalWorldOnB*cpd->m_appliedImpulse; + + //apply previous frames impulse on both bodies + body0->applyImpulse(totalImpulse, rel_pos1); + body1->applyImpulse(-totalImpulse, rel_pos2); + } + + } + } +} + +float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + float maxImpulse = 0.f; + + { + const int numpoints = manifoldPtr->getNumContacts(); + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + + int j=i; + if (iter % 2) + j = numpoints-1-i; + else + j=i; + + btManifoldPoint& cp = manifoldPtr->getContactPoint(j); + if (cp.getDistance() <= 0.f) + { + + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + float impulse = cpd->m_contactSolverFunc( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + +float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0(); + btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1(); + + + { + const int numpoints = manifoldPtr->getNumContacts(); + + btVector3 color(0,1,0); + for (int i=0;i<numpoints ;i++) + { + + int j=i; + //if (iter % 2) + // j = numpoints-1-i; + + btManifoldPoint& cp = manifoldPtr->getContactPoint(j); + if (cp.getDistance() <= 0.f) + { + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + cpd->m_frictionSolverFunc( + *body0,*body1, + cp, + info); + + + } + } + + + } + return 0.f; +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h new file mode 100644 index 00000000000..e399a5cd734 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -0,0 +1,64 @@ +/* +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 SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H + +#include "btConstraintSolver.h" +class btIDebugDraw; + +#include "btContactConstraint.h" + + + +/// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses +/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com +/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) +/// Applies impulses for combined restitution and penetration recovery and to simulate friction +class btSequentialImpulseConstraintSolver : public btConstraintSolver +{ + float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer); + + ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + +public: + + btSequentialImpulseConstraintSolver(); + + ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody + ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + void setContactSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_contactDispatch[type0][type1] = func; + } + + ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody + ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType + void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_frictionDispatch[type0][type1] = func; + } + + virtual ~btSequentialImpulseConstraintSolver() {} + + virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + +}; + +#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp new file mode 100644 index 00000000000..bf92434b69b --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp @@ -0,0 +1,241 @@ +/* +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 "btSolve2LinearConstraint.h" + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btVector3.h" +#include "btJacobianEntry.h" + + +void btSolve2LinearConstraint::resolveUnilateralPairConstraint( + btRigidBody* body1, + btRigidBody* body2, + + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + btScalar len = fabs(normalA.length())-1.f; + if (fabs(len) >= SIMD_EPSILON) + return; + + ASSERT(len < SIMD_EPSILON); + + + //this jacobian entry could be re-used for all iterations + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + +// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + btScalar massTerm = 1.f / (invMassA + invMassB); + + + // calculate rhs (or error) terms + const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; + + + // dC/dv * dv = -C + + // jacobian * impulse = -error + // + + //impulse = jacobianInverse * -error + + // inverting 2x2 symmetric system (offdiagonal are equal!) + // + + + btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + + //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + //[a b] [d -c] + //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) + + //[jA nD] * [imp0] = [dv0] + //[nD jB] [imp1] [dv1] + +} + + + +void btSolve2LinearConstraint::resolveBilateralPairConstraint( + btRigidBody* body1, + btRigidBody* body2, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + btScalar len = fabs(normalA.length())-1.f; + if (fabs(len) >= SIMD_EPSILON) + return; + + ASSERT(len < SIMD_EPSILON); + + + //this jacobian entry could be re-used for all iterations + btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + + // calculate rhs (or error) terms + const btScalar dv0 = depthA * m_tau - vel0 * m_damping; + const btScalar dv1 = depthB * m_tau - vel1 * m_damping; + + // dC/dv * dv = -C + + // jacobian * impulse = -error + // + + //impulse = jacobianInverse * -error + + // inverting 2x2 symmetric system (offdiagonal are equal!) + // + + + btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); + + //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; + imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; + + //[a b] [d -c] + //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) + + //[jA nD] * [imp0] = [dv0] + //[nD jB] [imp1] [dv1] + + if ( imp0 > 0.0f) + { + if ( imp1 > 0.0f ) + { + //both positive + } + else + { + imp1 = 0.f; + + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.getDiagonal(); + if ( imp0 > 0.0f ) + { + } else + { + imp0 = 0.f; + } + } + } + else + { + imp0 = 0.f; + + imp1 = dv1 / jacB.getDiagonal(); + if ( imp1 <= 0.0f ) + { + imp1 = 0.f; + // now imp0>0 imp1<0 + imp0 = dv0 / jacA.getDiagonal(); + if ( imp0 > 0.0f ) + { + } else + { + imp0 = 0.f; + } + } else + { + } + } +} + + + +void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btMatrix3x3& invInertiaBWS, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1) +{ + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h new file mode 100644 index 00000000000..639e4c9433f --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -0,0 +1,106 @@ +/* +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 SOLVE_2LINEAR_CONSTRAINT_H +#define SOLVE_2LINEAR_CONSTRAINT_H + +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btVector3.h" + + +class btRigidBody; + + + +/// constraint class used for lateral tyre friction. +class btSolve2LinearConstraint +{ + btScalar m_tau; + btScalar m_damping; + +public: + + btSolve2LinearConstraint(btScalar tau,btScalar damping) + { + m_tau = tau; + m_damping = damping; + } + // + // solve unilateral constraint (equality, direct method) + // + void resolveUnilateralPairConstraint( + btRigidBody* body0, + btRigidBody* body1, + + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + + // + // solving 2x2 lcp problem (inequality, direct solution ) + // + void resolveBilateralPairConstraint( + btRigidBody* body0, + btRigidBody* body1, + const btMatrix3x3& world2A, + const btMatrix3x3& world2B, + + const btVector3& invInertiaADiag, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btVector3& invInertiaBDiag, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + + void resolveAngularConstraint( const btMatrix3x3& invInertiaAWS, + const btScalar invMassA, + const btVector3& linvelA,const btVector3& angvelA, + const btVector3& rel_posA1, + const btMatrix3x3& invInertiaBWS, + const btScalar invMassB, + const btVector3& linvelB,const btVector3& angvelB, + const btVector3& rel_posA2, + + btScalar depthA, const btVector3& normalA, + const btVector3& rel_posB1,const btVector3& rel_posB2, + btScalar depthB, const btVector3& normalB, + btScalar& imp0,btScalar& imp1); + + +}; + +#endif //SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp new file mode 100644 index 00000000000..ea7d5c8b903 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -0,0 +1,53 @@ +/* +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 "btTypedConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +static btRigidBody s_fixed(0, btTransform::getIdentity(),0); + +btTypedConstraint::btTypedConstraint() +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(s_fixed), +m_rbB(s_fixed), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); +} +btTypedConstraint::btTypedConstraint(btRigidBody& rbA) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(s_fixed), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + +} + + +btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(rbB), +m_appliedImpulse(0.f) +{ + s_fixed.setMassProps(0.f,btVector3(0.f,0.f,0.f)); + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h new file mode 100644 index 00000000000..bc25eaa759c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -0,0 +1,90 @@ +/* +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 TYPED_CONSTRAINT_H +#define TYPED_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btScalar.h" + +///TypedConstraint is the baseclass for Bullet constraints and vehicles +class btTypedConstraint +{ + int m_userConstraintType; + int m_userConstraintId; + + +protected: + btRigidBody& m_rbA; + btRigidBody& m_rbB; + float m_appliedImpulse; + + +public: + + btTypedConstraint(); + virtual ~btTypedConstraint() {}; + btTypedConstraint(btRigidBody& rbA); + + btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB); + + virtual void buildJacobian() = 0; + + virtual void solveConstraint(btScalar timeStep) = 0; + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + btRigidBody& getRigidBodyA() + { + return m_rbA; + } + btRigidBody& getRigidBodyB() + { + return m_rbB; + } + + int getUserConstraintType() const + { + return m_userConstraintType ; + } + + void setUserConstraintType(int userConstraintType) + { + m_userConstraintType = userConstraintType; + }; + + void setUserConstraintId(int uid) + { + m_userConstraintId = uid; + } + + int getUserConstraintId() + { + return m_userConstraintId; + } + float getAppliedImpulse() + { + return m_appliedImpulse; + } +}; + +#endif //TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp new file mode 100644 index 00000000000..feb1d2823f1 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -0,0 +1,746 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btDiscreteDynamicsWorld.h" + + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include <LinearMath/btTransformUtil.h> + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + +//for debug rendering +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "LinearMath/btIDebugDraw.h" + + + +//vehicle +#include "BulletDynamics/Vehicle/btRaycastVehicle.h" +#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" +#include "BulletDynamics/Vehicle/btWheelInfo.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + + + +#include <algorithm> + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld() +:btDynamicsWorld(), +m_constraintSolver(new btSequentialImpulseConstraintSolver), +m_debugDrawer(0), +m_gravity(0,-10,0), +m_localTime(1.f/60.f), +m_profileTimings(0) +{ + m_islandManager = new btSimulationIslandManager(); + m_ownsIslandManager = true; + m_ownsConstraintSolver = true; + +} + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) +:btDynamicsWorld(dispatcher,pairCache), +m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), +m_debugDrawer(0), +m_gravity(0,-10,0), +m_localTime(1.f/60.f), +m_profileTimings(0) +{ + m_islandManager = new btSimulationIslandManager(); + m_ownsIslandManager = true; + m_ownsConstraintSolver = (constraintSolver==0); +} + + +btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() +{ + //only delete it when we created it + if (m_ownsIslandManager) + delete m_islandManager; + if (m_ownsConstraintSolver) + delete m_constraintSolver; +} + +void btDiscreteDynamicsWorld::saveKinematicState(float timeStep) +{ + + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + btTransform predictedTrans; + if (body->GetActivationState() != ISLAND_SLEEPING) + { + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } + } + } + } +} + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ + //todo: iterate over awake simulation islands! + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(255.f,255.f,255.f); + switch(colObj->GetActivationState()) + { + case ACTIVE_TAG: + color = btVector3(255.f,255.f,255.f); + case ISLAND_SLEEPING: + color = btVector3(0.f,255.f,0.f); + case WANTS_DEACTIVATION: + color = btVector3(0.f,255.f,255.f); + case DISABLE_DEACTIVATION: + color = btVector3(255.f,0.f,0.f); + case DISABLE_SIMULATION: + color = btVector3(255.f,255.f,0.f); + default: + { + color = btVector3(255.f,0.f,0.f); + } + }; + + debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color); + } + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + { + if (body->GetActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,body->getLinearVelocity(),body->getAngularVelocity(),m_localTime,interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } + } + +} + + +int btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +{ + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int( m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = timeStep; + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + + //process some debugging flags + if (getDebugDrawer()) + { + gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (!btFuzzyZero(timeStep) && numSimulationSubSteps) + { + + saveKinematicState(fixedTimeStep); + + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + + for (int i=0;i<clampedSimulationSteps;i++) + { + internalSingleStepSimulation(fixedTimeStep); + } + + } + + synchronizeMotionStates(); + + return numSimulationSubSteps; +} + +void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep) +{ + + startProfiling(timeStep); + + ///update aabbs information + updateAabbs(); + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + ///perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + btContactSolverInfo infoGlobal; + infoGlobal.m_timeStep = timeStep; + + ///solve non-contact constraints + solveNoncontactConstraints(infoGlobal); + + ///solve contact constraints + solveContactConstraints(infoGlobal); + + ///update vehicle simulation + updateVehicles(timeStep); + + ///CallbackTriggers(); + + ///integrate transforms + integrateTransforms(timeStep); + + updateActivationState( timeStep ); + + + +} + +void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->setGravity(gravity); + } + } +} + + +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + removeCollisionObject(body); +} + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +{ + body->setGravity(m_gravity); + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); + short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter; + short collisionFilterMask = isDynamic? btBroadphaseProxy::AllFilter : btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter; + + addCollisionObject(body,collisionFilterGroup,collisionFilterMask); +} + + +void btDiscreteDynamicsWorld::updateVehicles(float timeStep) +{ + BEGIN_PROFILE("updateVehicles"); + + for (unsigned int i=0;i<m_vehicles.size();i++) + { + btRaycastVehicle* vehicle = m_vehicles[i]; + vehicle->updateVehicle( timeStep); + } + END_PROFILE("updateVehicles"); +} + +void btDiscreteDynamicsWorld::updateActivationState(float timeStep) +{ + BEGIN_PROFILE("updateActivationState"); + + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->updateDeactivation(timeStep); + + if (body->wantsSleeping()) + { + if (body->GetActivationState() == ACTIVE_TAG) + body->SetActivationState( WANTS_DEACTIVATION ); + } else + { + if (body->GetActivationState() != DISABLE_DEACTIVATION) + body->SetActivationState( ACTIVE_TAG ); + } + } + } + END_PROFILE("updateActivationState"); +} + +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint) +{ + m_constraints.push_back(constraint); +} + +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +{ + std::vector<btTypedConstraint*>::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint); + if (!(cit==m_constraints.end())) + { + m_constraints.erase(cit); + } +} + +void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) +{ + m_vehicles.push_back(vehicle); +} + +void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) +{ + std::vector<btRaycastVehicle*>::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle); + if (!(vit==m_vehicles.end())) + { + m_vehicles.erase(vit); + } +} + + +void btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo) +{ + + BEGIN_PROFILE("solveContactConstraints"); + + struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback + { + + btContactSolverInfo& m_solverInfo; + btConstraintSolver* m_solver; + btIDebugDraw* m_debugDrawer; + + InplaceSolverIslandCallback( + btContactSolverInfo& solverInfo, + btConstraintSolver* solver, + btIDebugDraw* debugDrawer) + :m_solverInfo(solverInfo), + m_solver(solver), + m_debugDrawer(debugDrawer) + { + + } + + virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) + { + m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); + } + + }; + + + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, m_debugDrawer); + + + /// solve all the contact points and contact friction + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); + + END_PROFILE("solveContactConstraints"); + +} + + +void btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo) +{ + BEGIN_PROFILE("solveNoncontactConstraints"); + + int i; + int numConstraints = m_constraints.size(); + + ///constraint preparation: building jacobians + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + constraint->buildJacobian(); + } + + //solve the regular non-contact constraints (point 2 point, hinge, generic d6) + for (int g=0;g<solverInfo.m_numIterations;g++) + { + // + // constraint solving + // + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + constraint->solveConstraint( solverInfo.m_timeStep ); + } + } + + END_PROFILE("solveNoncontactConstraints"); + +} + +void btDiscreteDynamicsWorld::calculateSimulationIslands() +{ + BEGIN_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + int i; + int numConstraints = m_constraints.size(); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + if (colObj0->IsActive() || colObj1->IsActive()) + { + + getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + + END_PROFILE("calculateSimulationIslands"); + +} + +static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color) +{ + + btVector3 halfExtents = (to-from)* 0.5f; + btVector3 center = (to+from) *0.5f; + int i,j; + + btVector3 edgecoord(1.f,1.f,1.f),pa,pb; + for (i=0;i<4;i++) + { + for (j=0;j<3;j++) + { + pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pa+=center; + + int othercoord = j%3; + edgecoord[othercoord]*=-1.f; + pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pb+=center; + + debugDrawer->drawLine(pa,pb,color); + } + edgecoord = btVector3(-1.f,-1.f,-1.f); + if (i<3) + edgecoord[i]*=-1.f; + } + + +} + +void btDiscreteDynamicsWorld::updateAabbs() +{ + BEGIN_PROFILE("updateAabbs"); + + btVector3 colorvec(1,0,0); + btTransform predictedTrans; + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + // if (body->IsActive() && (!body->IsStatic())) + { + btPoint3 minAabb,maxAabb; + colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); + btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache; + bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec); + } + } + } + } + + END_PROFILE("updateAabbs"); +} + +void btDiscreteDynamicsWorld::integrateTransforms(float timeStep) +{ + BEGIN_PROFILE("integrateTransforms"); + btTransform predictedTrans; + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + if (body->IsActive() && (!body->isStaticOrKinematicObject())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + body->proceedToTransform( predictedTrans); + } + } + } + END_PROFILE("integrateTransforms"); +} + + + +void btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep) +{ + BEGIN_PROFILE("predictUnconstraintMotion"); + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + if (!body->isStaticOrKinematicObject()) + { + if (body->IsActive()) + { + body->applyForces( timeStep); + body->integrateVelocities( timeStep); + body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + } + } + } + } + END_PROFILE("predictUnconstraintMotion"); +} + + +void btDiscreteDynamicsWorld::startProfiling(float timeStep) +{ + #ifdef USE_QUICKPROF + + + //toggle btProfiler + if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings) + { + if (!m_profileTimings) + { + m_profileTimings = 1; + // To disable profiling, simply comment out the following line. + static int counter = 0; + + char filename[128]; + sprintf(filename,"quickprof_bullet_timings%i.csv",counter++); + btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS + } else + { + btProfiler::endProfilingCycle(); + } + + } else + { + if (m_profileTimings) + { + btProfiler::endProfilingCycle(); + + m_profileTimings = 0; + btProfiler::destroy(); + } + } +#endif //USE_QUICKPROF +} + + + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; + +public: + + DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) + : m_debugDrawer(debugDrawer), + m_worldTrans(worldTrans), + m_color(color) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + processTriangle(triangle,partId,triangleIndex); + } + + virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) + { + btVector3 wv0,wv1,wv2; + wv0 = m_worldTrans*triangle[0]; + wv1 = m_worldTrans*triangle[1]; + wv2 = m_worldTrans*triangle[2]; + m_debugDrawer->drawLine(wv0,wv1,m_color); + m_debugDrawer->drawLine(wv1,wv2,m_color); + m_debugDrawer->drawLine(wv2,wv0,m_color); + } +}; + + + +void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) +{ + + if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); + for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* colShape = compoundShape->getChildShape(i); + debugDrawObject(worldTransform*childTrans,colShape,color); + } + + } else + { + switch (shape->getShapeType()) + { + + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color); + getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color); + getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color); + //drawSphere + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + float radius = coneShape->getRadius();//+coneShape->getMargin(); + float height = coneShape->getHeight();//+coneShape->getMargin(); + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(radius,0,-0.5*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(-radius,0,-0.5*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,radius,-0.5*height),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,0,0.5*height),start+worldTransform.getBasis() * btVector3(0,-radius,-0.5*height),color); + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + float radius = cylinder->getRadius(); + float halfHeight = cylinder->getHalfExtents()[upAxis]; + btVector3 start = worldTransform.getOrigin(); + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = halfHeight; + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + break; + } + default: + { + + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape; + //btVector3 aabbMax(1e30f,1e30f,1e30f); + //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + + //todo pass camera, for some culling + btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(1e30f,1e30f,1e30f); + btVector3 aabbMin(-1e30f,-1e30f,-1e30f); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + for (i=0;i<polyshape->getNumEdges();i++) + { + btPoint3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + + } + + + } + } + } + } +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h new file mode 100644 index 00000000000..77e4ada645c --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -0,0 +1,142 @@ +/* +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; +struct btContactSolverInfo; +class btRaycastVehicle; +class btIDebugDraw; + +#include <vector> + +///btDiscreteDynamicsWorld provides discrete rigid body simulation +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btDiscreteDynamicsWorld : public btDynamicsWorld +{ +protected: + + btConstraintSolver* m_constraintSolver; + + btSimulationIslandManager* m_islandManager; + + std::vector<btTypedConstraint*> m_constraints; + + btIDebugDraw* m_debugDrawer; + + btVector3 m_gravity; + + //for variable timesteps + float m_localTime; + //for variable timesteps + + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + + std::vector<btRaycastVehicle*> m_vehicles; + + int m_profileTimings; + + void predictUnconstraintMotion(float timeStep); + + void integrateTransforms(float timeStep); + + void calculateSimulationIslands(); + + void solveNoncontactConstraints(btContactSolverInfo& solverInfo); + + void solveContactConstraints(btContactSolverInfo& solverInfo); + + void updateActivationState(float timeStep); + + void updateVehicles(float timeStep); + + void startProfiling(float timeStep); + + virtual void internalSingleStepSimulation( float timeStep); + + void synchronizeMotionStates(); + + void saveKinematicState(float timeStep); + + +public: + + + ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those + btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver=0); + + ///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor. + btDiscreteDynamicsWorld(); + + virtual ~btDiscreteDynamicsWorld(); + + ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's + virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f); + + virtual void updateAabbs(); + + void addConstraint(btTypedConstraint* constraint); + + void removeConstraint(btTypedConstraint* constraint); + + void addVehicle(btRaycastVehicle* vehicle); + + void removeVehicle(btRaycastVehicle* vehicle); + + btSimulationIslandManager* getSimulationIslandManager() + { + return m_islandManager; + } + + const btSimulationIslandManager* getSimulationIslandManager() const + { + return m_islandManager; + } + + btCollisionWorld* getCollisionWorld() + { + return this; + } + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + } + + virtual btIDebugDraw* getDebugDrawer() + { + return m_debugDrawer; + } + + virtual void setGravity(const btVector3& gravity); + + virtual void addRigidBody(btRigidBody* body); + + virtual void removeRigidBody(btRigidBody* body); + + void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + + +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h new file mode 100644 index 00000000000..8a872264c22 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -0,0 +1,65 @@ +/* +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" +class btTypedConstraint; + +///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous +class btDynamicsWorld : public btCollisionWorld +{ + public: + + btDynamicsWorld() + { + } + + btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) + :btCollisionWorld(dispatcher,pairCache) + { + } + + virtual ~btDynamicsWorld() + { + } + + ///stepSimulation proceeds the simulation over timeStep units + ///if maxSubSteps > 0, it will interpolate time steps + virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f)=0; + + virtual void updateAabbs() = 0; + + virtual void addConstraint(btTypedConstraint* constraint) {}; + + virtual void removeConstraint(btTypedConstraint* constraint) {}; + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; + + virtual btIDebugDraw* getDebugDrawer() = 0; + + //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 void addRigidBody(btRigidBody* body) = 0; + + virtual void removeRigidBody(btRigidBody* body) = 0; + +}; + +#endif //BT_DYNAMICS_WORLD_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp new file mode 100644 index 00000000000..9e7c4978c31 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -0,0 +1,265 @@ +/* +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 "btRigidBody.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btMinMax.h" +#include <LinearMath/btTransformUtil.h> +#include <LinearMath/btMotionState.h> + +float gLinearAirDamping = 1.f; +//'temporarily' global variables +float gDeactivationTime = 2.f; +bool gDisableDeactivation = false; + +float gLinearSleepingTreshold = 0.8f; +float gAngularSleepingTreshold = 1.0f; +static int uniqueId = 0; + +btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) +: + m_gravity(0.0f, 0.0f, 0.0f), + m_totalForce(0.0f, 0.0f, 0.0f), + m_totalTorque(0.0f, 0.0f, 0.0f), + m_linearVelocity(0.0f, 0.0f, 0.0f), + m_angularVelocity(0.f,0.f,0.f), + m_linearDamping(0.f), + m_angularDamping(0.5f), + m_optionalMotionState(motionState), + m_contactSolverType(0), + m_frictionSolverType(0) +{ + + motionState->getWorldTransform(m_worldTransform); + + m_interpolationWorldTransform = m_worldTransform; + + //moved to btCollisionObject + m_friction = friction; + m_restitution = restitution; + + m_collisionShape = collisionShape; + m_debugBodyId = uniqueId++; + + //m_internalOwner is to allow upcasting from collision object to rigid body + m_internalOwner = this; + + setMassProps(mass, localInertia); + setDamping(linearDamping, angularDamping); + updateInertiaTensor(); + +} + + +btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) +: + m_gravity(0.0f, 0.0f, 0.0f), + m_totalForce(0.0f, 0.0f, 0.0f), + m_totalTorque(0.0f, 0.0f, 0.0f), + m_linearVelocity(0.0f, 0.0f, 0.0f), + m_angularVelocity(0.f,0.f,0.f), + m_linearDamping(0.f), + m_angularDamping(0.5f), + m_optionalMotionState(0), + m_contactSolverType(0), + m_frictionSolverType(0) +{ + + m_worldTransform = worldTransform; + m_interpolationWorldTransform = m_worldTransform; + + //moved to btCollisionObject + m_friction = friction; + m_restitution = restitution; + + m_collisionShape = collisionShape; + m_debugBodyId = uniqueId++; + + //m_internalOwner is to allow upcasting from collision object to rigid body + m_internalOwner = this; + + setMassProps(mass, localInertia); + setDamping(linearDamping, angularDamping); + updateInertiaTensor(); + +} + + + + + +void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) const +{ + btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); +} + +void btRigidBody::saveKinematicState(btScalar timeStep) +{ + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (timeStep != 0.f) + { + //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform + if (getMotionState()) + getMotionState()->getWorldTransform(m_worldTransform); + btVector3 linVel,angVel; + btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); + m_interpolationWorldTransform = m_worldTransform; + //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); + } +} + +void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const +{ + getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); +} + + + + +void btRigidBody::setGravity(const btVector3& acceleration) +{ + if (m_inverseMass != 0.0f) + { + m_gravity = acceleration * (1.0f / m_inverseMass); + } +} + + + + + + +void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) +{ + m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f); + m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f); +} + + + +#include <stdio.h> + + +void btRigidBody::applyForces(btScalar step) +{ + if (isStaticOrKinematicObject()) + return; + + applyCentralForce(m_gravity); + + m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f); + m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f); + +#define FORCE_VELOCITY_DAMPING 1 +#ifdef FORCE_VELOCITY_DAMPING + float speed = m_linearVelocity.length(); + if (speed < m_linearDamping) + { + float dampVel = 0.005f; + if (speed > dampVel) + { + btVector3 dir = m_linearVelocity.normalized(); + m_linearVelocity -= dir * dampVel; + } else + { + m_linearVelocity.setValue(0.f,0.f,0.f); + } + } + + float angSpeed = m_angularVelocity.length(); + if (angSpeed < m_angularDamping) + { + float angDampVel = 0.005f; + if (angSpeed > angDampVel) + { + btVector3 dir = m_angularVelocity.normalized(); + m_angularVelocity -= dir * angDampVel; + } else + { + m_angularVelocity.setValue(0.f,0.f,0.f); + } + } +#endif //FORCE_VELOCITY_DAMPING + +} + +void btRigidBody::proceedToTransform(const btTransform& newTrans) +{ + setCenterOfMassTransform( newTrans ); +} + + +void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) +{ + if (mass == 0.f) + { + m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; + m_inverseMass = 0.f; + } else + { + m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); + m_inverseMass = 1.0f / mass; + } + + m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f, + inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f, + inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f); + +} + + + +void btRigidBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); +} + + +void btRigidBody::integrateVelocities(btScalar step) +{ + if (isStaticOrKinematicObject()) + return; + + m_linearVelocity += m_totalForce * (m_inverseMass * step); + m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; + +#define MAX_ANGVEL SIMD_HALF_PI + /// clamp angular velocity. collision calculations will fail on higher angular velocities + float angvel = m_angularVelocity.length(); + if (angvel*step > MAX_ANGVEL) + { + m_angularVelocity *= (MAX_ANGVEL/step) /angvel; + } + + clearForces(); +} + +btQuaternion btRigidBody::getOrientation() const +{ + btQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; +} + + +void btRigidBody::setCenterOfMassTransform(const btTransform& xform) +{ + m_interpolationWorldTransform = m_worldTransform; + m_worldTransform = xform; + updateInertiaTensor(); +} + + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h new file mode 100644 index 00000000000..c6d3bd50a35 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -0,0 +1,316 @@ +/* +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 RIGIDBODY_H +#define RIGIDBODY_H + +#include <vector> +#include <LinearMath/btPoint3.h> +#include <LinearMath/btTransform.h> +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" + + +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +class btCollisionShape; +class btMotionState; + + + +extern float gLinearAirDamping; +extern bool gUseEpa; + +extern float gDeactivationTime; +extern bool gDisableDeactivation; +extern float gLinearSleepingTreshold; +extern float gAngularSleepingTreshold; + + +/// btRigidBody class for btRigidBody Dynamics +/// +class btRigidBody : public btCollisionObject +{ + + btMatrix3x3 m_invInertiaTensorWorld; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btScalar m_inverseMass; + + btVector3 m_gravity; + btVector3 m_invInertiaLocal; + btVector3 m_totalForce; + btVector3 m_totalTorque; + + btScalar m_linearDamping; + btScalar m_angularDamping; + + + //m_optionalMotionState allows to automatic synchronize the world transform for active objects + btMotionState* m_optionalMotionState; + +public: + + btRigidBody(float mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); + btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=0.f,btScalar angularDamping=0.f,btScalar friction=0.5f,btScalar restitution=0.f); + + void proceedToTransform(const btTransform& newTrans); + + ///to keep collision detection and dynamics separate we don't store a rigidbody pointer + ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast + static const btRigidBody* upcast(const btCollisionObject* colObj) + { + return (const btRigidBody*)colObj->m_internalOwner; + } + static btRigidBody* upcast(btCollisionObject* colObj) + { + return (btRigidBody*)colObj->m_internalOwner; + } + + /// continuous collision detection needs prediction + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) const; + + void saveKinematicState(btScalar step); + + + void applyForces(btScalar step); + + void setGravity(const btVector3& acceleration); + + const btVector3& getGravity() const + { + return m_gravity; + } + + void setDamping(btScalar lin_damping, btScalar ang_damping); + + inline const btCollisionShape* getCollisionShape() const { + return m_collisionShape; + } + + inline btCollisionShape* getCollisionShape() { + return m_collisionShape; + } + + void setMassProps(btScalar mass, const btVector3& inertia); + + btScalar getInvMass() const { return m_inverseMass; } + const btMatrix3x3& getInvInertiaTensorWorld() const { + return m_invInertiaTensorWorld; + } + + void integrateVelocities(btScalar step); + + void setCenterOfMassTransform(const btTransform& xform); + + void applyCentralForce(const btVector3& force) + { + m_totalForce += force; + } + + const btVector3& getInvInertiaDiagLocal() + { + return m_invInertiaLocal; + }; + + void setInvInertiaDiagLocal(const btVector3& diagInvInertia) + { + m_invInertiaLocal = diagInvInertia; + } + + void applyTorque(const btVector3& torque) + { + m_totalTorque += torque; + } + + void applyForce(const btVector3& force, const btVector3& rel_pos) + { + applyCentralForce(force); + applyTorque(rel_pos.cross(force)); + } + + void applyCentralImpulse(const btVector3& impulse) + { + m_linearVelocity += impulse * m_inverseMass; + } + + void applyTorqueImpulse(const btVector3& torque) + { + m_angularVelocity += m_invInertiaTensorWorld * torque; + } + + void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) + { + if (m_inverseMass != 0.f) + { + applyCentralImpulse(impulse); + applyTorqueImpulse(rel_pos.cross(impulse)); + } + } + + void clearForces() + { + m_totalForce.setValue(0.0f, 0.0f, 0.0f); + m_totalTorque.setValue(0.0f, 0.0f, 0.0f); + } + + void updateInertiaTensor(); + + const btPoint3& getCenterOfMassPosition() const { + return m_worldTransform.getOrigin(); + } + btQuaternion getOrientation() const; + + const btTransform& getCenterOfMassTransform() const { + return m_worldTransform; + } + const btVector3& getLinearVelocity() const { + return m_linearVelocity; + } + const btVector3& getAngularVelocity() const { + return m_angularVelocity; + } + + + inline void setLinearVelocity(const btVector3& lin_vel) + { + assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); + m_linearVelocity = lin_vel; + } + + inline void setAngularVelocity(const btVector3& ang_vel) { + assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); + { + m_angularVelocity = ang_vel; + } + } + + btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_linearVelocity + m_angularVelocity.cross(rel_pos); + + //for kinematic objects, we could also use use: + // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; + } + + void translate(const btVector3& v) + { + m_worldTransform.getOrigin() += v; + } + + + void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; + + + + + + inline float computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + { + btVector3 r0 = pos - getCenterOfMassPosition(); + + btVector3 c0 = (r0).cross(normal); + + btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); + + return m_inverseMass + normal.dot(vec); + + } + + inline float computeAngularImpulseDenominator(const btVector3& axis) const + { + btVector3 vec = axis * getInvInertiaTensorWorld(); + return axis.dot(vec); + } + + inline void updateDeactivation(float timeStep) + { + if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == DISABLE_DEACTIVATION)) + return; + + if ((getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) && + (getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold)) + { + m_deactivationTime += timeStep; + } else + { + m_deactivationTime=0.f; + SetActivationState(0); + } + + } + + inline bool wantsSleeping() + { + + if (GetActivationState() == DISABLE_DEACTIVATION) + return false; + + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == 0.f)) + return false; + + if ( (GetActivationState() == ISLAND_SLEEPING) || (GetActivationState() == WANTS_DEACTIVATION)) + return true; + + if (m_deactivationTime> gDeactivationTime) + { + return true; + } + return false; + } + + + + const btBroadphaseProxy* getBroadphaseProxy() const + { + return m_broadphaseHandle; + } + btBroadphaseProxy* getBroadphaseProxy() + { + return m_broadphaseHandle; + } + void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) + { + m_broadphaseHandle = broadphaseProxy; + } + + //btMotionState allows to automatic synchronize the world transform for active objects + btMotionState* getMotionState() + { + return m_optionalMotionState; + } + const btMotionState* getMotionState() const + { + return m_optionalMotionState; + } + void setMotionState(btMotionState* motionState) + { + m_optionalMotionState = motionState; + } + + //for experimental overriding of friction/contact solver func + int m_contactSolverType; + int m_frictionSolverType; + + + + int m_debugBodyId; +}; + + + +#endif + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp new file mode 100644 index 00000000000..53a088de750 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -0,0 +1,186 @@ +/* +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" + + +btSimpleDynamicsWorld::btSimpleDynamicsWorld() +:m_constraintSolver(new btSequentialImpulseConstraintSolver), +m_ownsConstraintSolver(true), +m_debugDrawer(0), +m_gravity(0,0,-10) +{ +} + +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) +:btDynamicsWorld(dispatcher,pairCache), +m_constraintSolver(constraintSolver), +m_ownsConstraintSolver(false), +m_debugDrawer(0), +m_gravity(0,0,-10) +{ + +} + + +btSimpleDynamicsWorld::~btSimpleDynamicsWorld() +{ + if (m_ownsConstraintSolver) + delete m_constraintSolver; +} + +int btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep) +{ + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + ///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->solveGroup(manifoldPtr, numManifolds,infoGlobal,m_debugDrawer); + } + + ///integrate transforms + integrateTransforms(timeStep); + + updateAabbs(); + + synchronizeMotionStates(); + + return 1; + +} + + +void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) +{ + m_gravity = gravity; + for (unsigned int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->setGravity(gravity); + } + } +} + +void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + removeCollisionObject(body); +} + +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) +{ + body->setGravity(m_gravity); + + addCollisionObject(body); +} + +void btSimpleDynamicsWorld::updateAabbs() +{ + btTransform predictedTrans; + for (unsigned 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())) + { + btPoint3 minAabb,maxAabb; + colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb); + btBroadphaseInterface* bp = getBroadphase(); + bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb); + } + } + } +} + +void btSimpleDynamicsWorld::integrateTransforms(float timeStep) +{ + btTransform predictedTrans; + for (unsigned 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(float timeStep) +{ + for (unsigned 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->applyForces( timeStep); + body->integrateVelocities( timeStep); + body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + } + } + } + } +} + + +void btSimpleDynamicsWorld::synchronizeMotionStates() +{ + //todo: iterate over awake simulation islands! + for (unsigned 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->m_worldTransform); + } + } + } + +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h new file mode 100644 index 00000000000..a65b56c4f57 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -0,0 +1,82 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SIMPLE_DYNAMICS_WORLD_H +#define BT_SIMPLE_DYNAMICS_WORLD_H + +#include "btDynamicsWorld.h" + +class btDispatcher; +class btOverlappingPairCache; +class btConstraintSolver; + +///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics +///It can be used for basic simulations, and as a starting point for porting Bullet +///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts. +///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btSimpleDynamicsWorld : public btDynamicsWorld +{ +protected: + + btConstraintSolver* m_constraintSolver; + + bool m_ownsConstraintSolver; + + btIDebugDraw* m_debugDrawer; + + void predictUnconstraintMotion(float timeStep); + + void integrateTransforms(float timeStep); + + btVector3 m_gravity; + +public: + + + ///this btSimpleDynamicsWorld constructor creates and owns dispatcher, broadphase pairCache and constraintSolver + btSimpleDynamicsWorld(); + + ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver + btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + + virtual ~btSimpleDynamicsWorld(); + + ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead + virtual int stepSimulation( float timeStep,int maxSubSteps=1, float fixedTimeStep=1.f/60.f); + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + }; + + virtual btIDebugDraw* getDebugDrawer() + { + return m_debugDrawer; + } + + virtual void setGravity(const btVector3& gravity); + + virtual void addRigidBody(btRigidBody* body); + + virtual void removeRigidBody(btRigidBody* body); + + virtual void updateAabbs(); + + void synchronizeMotionStates(); + +}; + +#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp new file mode 100644 index 00000000000..03841f99a70 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -0,0 +1,595 @@ +/* + * 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 "btRaycastVehicle.h" +#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" +#include "BulletDynamics/ConstraintSolver/btJacobianEntry.h" +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btVector3.h" +#include "btVehicleRaycaster.h" +#include "btWheelInfo.h" + + +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + + + +static btRigidBody s_fixedObject( 0,btTransform(btQuaternion(0,0,0,1)),0); + +btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) +:m_vehicleRaycaster(raycaster), +m_pitchControl(0.f) +{ + m_chassisBody = chassis; + m_indexRightAxis = 0; + m_indexUpAxis = 2; + m_indexForwardAxis = 1; + defaultInit(tuning); +} + + +void btRaycastVehicle::defaultInit(const btVehicleTuning& tuning) +{ + m_currentVehicleSpeedKmHour = 0.f; + m_steeringValue = 0.f; + +} + + + +btRaycastVehicle::~btRaycastVehicle() +{ +} + + +// +// basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed +// +btWheelInfo& btRaycastVehicle::addWheel( const btVector3& connectionPointCS, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS, btScalar suspensionRestLength, btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel) +{ + + btWheelInfoConstructionInfo ci; + + ci.m_chassisConnectionCS = connectionPointCS; + ci.m_wheelDirectionCS = wheelDirectionCS0; + ci.m_wheelAxleCS = wheelAxleCS; + ci.m_suspensionRestLength = suspensionRestLength; + ci.m_wheelRadius = wheelRadius; + ci.m_suspensionStiffness = tuning.m_suspensionStiffness; + ci.m_wheelsDampingCompression = tuning.m_suspensionCompression; + ci.m_wheelsDampingRelaxation = tuning.m_suspensionDamping; + ci.m_frictionSlip = tuning.m_frictionSlip; + ci.m_bIsFrontWheel = isFrontWheel; + ci.m_maxSuspensionTravelCm = tuning.m_maxSuspensionTravelCm; + + m_wheelInfo.push_back( btWheelInfo(ci)); + + btWheelInfo& wheel = m_wheelInfo[getNumWheels()-1]; + + updateWheelTransformsWS( wheel ); + updateWheelTransform(getNumWheels()-1); + return wheel; +} + + + + +const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const +{ + assert(wheelIndex < getNumWheels()); + const btWheelInfo& wheel = m_wheelInfo[wheelIndex]; + return wheel.m_worldTransform; + +} + +void btRaycastVehicle::updateWheelTransform( int wheelIndex ) +{ + + btWheelInfo& wheel = m_wheelInfo[ wheelIndex ]; + updateWheelTransformsWS(wheel); + btVector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; + const btVector3& right = wheel.m_raycastInfo.m_wheelAxleWS; + btVector3 fwd = up.cross(right); + fwd = fwd.normalize(); + //rotate around steering over de wheelAxleWS + float steering = wheel.m_steering; + + btQuaternion steeringOrn(up,steering);//wheel.m_steering); + btMatrix3x3 steeringMat(steeringOrn); + + btQuaternion rotatingOrn(right,wheel.m_rotation); + btMatrix3x3 rotatingMat(rotatingOrn); + + btMatrix3x3 basis2( + right[0],fwd[0],up[0], + right[1],fwd[1],up[1], + right[2],fwd[2],up[2] + ); + + wheel.m_worldTransform.setBasis(steeringMat * rotatingMat * basis2); + wheel.m_worldTransform.setOrigin( + wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength + ); +} + +void btRaycastVehicle::resetSuspension() +{ + + std::vector<btWheelInfo>::iterator wheelIt; + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + btWheelInfo& wheel = *wheelIt; + wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); + wheel.m_suspensionRelativeVelocity = 0.0f; + + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + //wheel_info.setContactFriction(0.0f); + wheel.m_clippedInvContactDotSuspension = 1.0f; + } +} + +void btRaycastVehicle::updateWheelTransformsWS(btWheelInfo& wheel ) +{ + wheel.m_raycastInfo.m_isInContact = false; + + const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform(); + + wheel.m_raycastInfo.m_hardPointWS = chassisTrans( wheel.m_chassisConnectionPointCS ); + wheel.m_raycastInfo.m_wheelDirectionWS = chassisTrans.getBasis() * wheel.m_wheelDirectionCS ; + wheel.m_raycastInfo.m_wheelAxleWS = chassisTrans.getBasis() * wheel.m_wheelAxleCS; +} + +btScalar btRaycastVehicle::rayCast(btWheelInfo& wheel) +{ + updateWheelTransformsWS( wheel ); + + + btScalar depth = -1; + + btScalar raylen = wheel.getSuspensionRestLength()+wheel.m_wheelsRadius; + + btVector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen); + const btVector3& source = wheel.m_raycastInfo.m_hardPointWS; + wheel.m_raycastInfo.m_contactPointWS = source + rayvector; + const btVector3& target = wheel.m_raycastInfo.m_contactPointWS; + + btScalar param = 0.f; + + btVehicleRaycaster::btVehicleRaycasterResult rayResults; + + void* object = m_vehicleRaycaster->castRay(source,target,rayResults); + + wheel.m_raycastInfo.m_groundObject = 0; + + if (object) + { + param = rayResults.m_distFraction; + depth = raylen * rayResults.m_distFraction; + wheel.m_raycastInfo.m_contactNormalWS = rayResults.m_hitNormalInWorld; + wheel.m_raycastInfo.m_isInContact = true; + + wheel.m_raycastInfo.m_groundObject = &s_fixedObject;//todo for driving on dynamic/movable objects!; + //wheel.m_raycastInfo.m_groundObject = object; + + + btScalar hitDistance = param*raylen; + wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelsRadius; + //clamp on max suspension travel + + float minSuspensionLength = wheel.getSuspensionRestLength() - wheel.m_maxSuspensionTravelCm*0.01f; + float maxSuspensionLength = wheel.getSuspensionRestLength()+ wheel.m_maxSuspensionTravelCm*0.01f; + if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength; + } + if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) + { + wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength; + } + + wheel.m_raycastInfo.m_contactPointWS = rayResults.m_hitPointInWorld; + + btScalar denominator= wheel.m_raycastInfo.m_contactNormalWS.dot( wheel.m_raycastInfo.m_wheelDirectionWS ); + + btVector3 chassis_velocity_at_contactPoint; + btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition(); + + chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos); + + btScalar projVel = wheel.m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); + + if ( denominator >= -0.1f) + { + wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_clippedInvContactDotSuspension = 1.0f / 0.1f; + } + else + { + btScalar inv = -1.f / denominator; + wheel.m_suspensionRelativeVelocity = projVel * inv; + wheel.m_clippedInvContactDotSuspension = inv; + } + + } else + { + //put wheel info as in rest position + wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength(); + wheel.m_suspensionRelativeVelocity = 0.0f; + wheel.m_raycastInfo.m_contactNormalWS = - wheel.m_raycastInfo.m_wheelDirectionWS; + wheel.m_clippedInvContactDotSuspension = 1.0f; + } + + return depth; +} + + +void btRaycastVehicle::updateVehicle( btScalar step ) +{ + + m_currentVehicleSpeedKmHour = 3.6f * getRigidBody()->getLinearVelocity().length(); + + const btTransform& chassisTrans = getRigidBody()->getCenterOfMassTransform(); + btVector3 forwardW ( + chassisTrans.getBasis()[0][m_indexForwardAxis], + chassisTrans.getBasis()[1][m_indexForwardAxis], + chassisTrans.getBasis()[2][m_indexForwardAxis]); + + if (forwardW.dot(getRigidBody()->getLinearVelocity()) < 0.f) + { + m_currentVehicleSpeedKmHour *= -1.f; + } + + // + // simulate suspension + // + std::vector<btWheelInfo>::iterator wheelIt; + int i=0; + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++,i++) + { + btScalar depth; + depth = rayCast( *wheelIt ); + } + + updateSuspension(step); + + + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + //apply suspension force + btWheelInfo& wheel = *wheelIt; + + float suspensionForce = wheel.m_wheelsSuspensionForce; + + float gMaxSuspensionForce = 6000.f; + if (suspensionForce > gMaxSuspensionForce) + { + suspensionForce = gMaxSuspensionForce; + } + btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step; + btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition(); + + getRigidBody()->applyImpulse(impulse, relpos); + + } + + + + updateFriction( step); + + + for (wheelIt = m_wheelInfo.begin(); + !(wheelIt == m_wheelInfo.end());wheelIt++) + { + btWheelInfo& wheel = *wheelIt; + btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition(); + btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos ); + + if (wheel.m_raycastInfo.m_isInContact) + { + btVector3 fwd ( + getRigidBody()->getCenterOfMassTransform().getBasis()[0][m_indexForwardAxis], + getRigidBody()->getCenterOfMassTransform().getBasis()[1][m_indexForwardAxis], + getRigidBody()->getCenterOfMassTransform().getBasis()[2][m_indexForwardAxis]); + + btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS); + fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj; + + btScalar proj2 = fwd.dot(vel); + + wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius); + wheel.m_rotation += wheel.m_deltaRotation; + + } else + { + wheel.m_rotation += wheel.m_deltaRotation; + } + + wheel.m_deltaRotation *= 0.99f;//damping of rotation when not in contact + + } + + + +} + + +void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel) +{ + assert(wheel>=0 && wheel < getNumWheels()); + + btWheelInfo& wheelInfo = getWheelInfo(wheel); + wheelInfo.m_steering = steering; +} + + + +btScalar btRaycastVehicle::getSteeringValue(int wheel) const +{ + return getWheelInfo(wheel).m_steering; +} + + +void btRaycastVehicle::applyEngineForce(btScalar force, int wheel) +{ + assert(wheel>=0 && wheel < getNumWheels()); + btWheelInfo& wheelInfo = getWheelInfo(wheel); + wheelInfo.m_engineForce = force; +} + + +const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const +{ + ASSERT((index >= 0) && (index < getNumWheels())); + + return m_wheelInfo[index]; +} + +btWheelInfo& btRaycastVehicle::getWheelInfo(int index) +{ + ASSERT((index >= 0) && (index < getNumWheels())); + + return m_wheelInfo[index]; +} + +void btRaycastVehicle::setBrake(float brake,int wheelIndex) +{ + ASSERT((wheelIndex >= 0) && (wheelIndex < getNumWheels())); + getWheelInfo(wheelIndex).m_brake; +} + + +void btRaycastVehicle::updateSuspension(btScalar deltaTime) +{ + + btScalar chassisMass = 1.f / m_chassisBody->getInvMass(); + + for (int w_it=0; w_it<getNumWheels(); w_it++) + { + btWheelInfo &wheel_info = m_wheelInfo[w_it]; + + if ( wheel_info.m_raycastInfo.m_isInContact ) + { + btScalar force; + // Spring + { + btScalar susp_length = wheel_info.getSuspensionRestLength(); + btScalar current_length = wheel_info.m_raycastInfo.m_suspensionLength; + + btScalar length_diff = (susp_length - current_length); + + force = wheel_info.m_suspensionStiffness + * length_diff * wheel_info.m_clippedInvContactDotSuspension; + } + + // Damper + { + btScalar projected_rel_vel = wheel_info.m_suspensionRelativeVelocity; + { + btScalar susp_damping; + if ( projected_rel_vel < 0.0f ) + { + susp_damping = wheel_info.m_wheelsDampingCompression; + } + else + { + susp_damping = wheel_info.m_wheelsDampingRelaxation; + } + force -= susp_damping * projected_rel_vel; + } + } + + // RESULT + wheel_info.m_wheelsSuspensionForce = force * chassisMass; + if (wheel_info.m_wheelsSuspensionForce < 0.f) + { + wheel_info.m_wheelsSuspensionForce = 0.f; + } + } + else + { + wheel_info.m_wheelsSuspensionForce = 0.0f; + } + } + +} + +float sideFrictionStiffness2 = 1.0f; +void btRaycastVehicle::updateFriction(btScalar timeStep) +{ + + //calculate the impulse, so that the wheels don't move sidewards + int numWheel = getNumWheels(); + if (!numWheel) + return; + + + btVector3* forwardWS = new btVector3[numWheel]; + btVector3* axle = new btVector3[numWheel]; + btScalar* forwardImpulse = new btScalar[numWheel]; + btScalar* sideImpulse = new btScalar[numWheel]; + + int numWheelsOnGround = 0; + + + //collapse all those loops into one! + for (int i=0;i<getNumWheels();i++) + { + btWheelInfo& wheelInfo = m_wheelInfo[i]; + class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + if (groundObject) + numWheelsOnGround++; + sideImpulse[i] = 0.f; + forwardImpulse[i] = 0.f; + + } + + { + + for (int i=0;i<getNumWheels();i++) + { + + btWheelInfo& wheelInfo = m_wheelInfo[i]; + + class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + + if (groundObject) + { + + const btTransform& wheelTrans = getWheelTransformWS( i ); + + btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); + axle[i] = btVector3( + wheelBasis0[0][m_indexRightAxis], + wheelBasis0[1][m_indexRightAxis], + wheelBasis0[2][m_indexRightAxis]); + + const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; + btScalar proj = axle[i].dot(surfNormalWS); + axle[i] -= surfNormalWS * proj; + axle[i] = axle[i].normalize(); + + forwardWS[i] = surfNormalWS.cross(axle[i]); + forwardWS[i].normalize(); + + + resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, + *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, + 0.f, axle[i],sideImpulse[i],timeStep); + + sideImpulse[i] *= sideFrictionStiffness2; + + } + + + } + } + + btScalar sideFactor = 1.f; + btScalar fwdFactor = 0.5; + + bool sliding = false; + { + for (int wheel =0;wheel <getNumWheels();wheel++) + { + btWheelInfo& wheelInfo = m_wheelInfo[wheel]; + class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; + + + forwardImpulse[wheel] = 0.f; + m_wheelInfo[wheel].m_skidInfo= 1.f; + + if (groundObject) + { + m_wheelInfo[wheel].m_skidInfo= 1.f; + + btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; + btScalar maximpSide = maximp; + + btScalar maximpSquared = maximp * maximpSide; + + forwardImpulse[wheel] = wheelInfo.m_engineForce* timeStep; + + float x = (forwardImpulse[wheel] ) * fwdFactor; + float y = (sideImpulse[wheel] ) * sideFactor; + + float impulseSquared = (x*x + y*y); + + if (impulseSquared > maximpSquared) + { + sliding = true; + + btScalar factor = maximp / btSqrt(impulseSquared); + + m_wheelInfo[wheel].m_skidInfo *= factor; + } + } + + } + } + + + + + if (sliding) + { + for (int wheel = 0;wheel < getNumWheels(); wheel++) + { + if (sideImpulse[wheel] != 0.f) + { + if (m_wheelInfo[wheel].m_skidInfo< 1.f) + { + forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; + } + } + } + } + + // apply the impulses + { + for (int wheel = 0;wheel<getNumWheels() ; wheel++) + { + btWheelInfo& wheelInfo = m_wheelInfo[wheel]; + + btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - + m_chassisBody->getCenterOfMassPosition(); + + if (forwardImpulse[wheel] != 0.f) + { + m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos); + } + if (sideImpulse[wheel] != 0.f) + { + class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; + + btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - + groundObject->getCenterOfMassPosition(); + + + btVector3 sideImp = axle[wheel] * sideImpulse[wheel]; + + rel_pos[2] *= wheelInfo.m_rollInfluence; + m_chassisBody->applyImpulse(sideImp,rel_pos); + + //apply friction impulse on the ground + groundObject->applyImpulse(-sideImp,rel_pos2); + } + } + } + + delete []forwardWS; + delete [] axle; + delete[]forwardImpulse; + delete[] sideImpulse; +} diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h new file mode 100644 index 00000000000..8468bc52016 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -0,0 +1,167 @@ +/* + * 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. +*/ +#ifndef RAYCASTVEHICLE_H +#define RAYCASTVEHICLE_H + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + +#include "btWheelInfo.h" + +struct btVehicleRaycaster; +class btVehicleTuning; + +///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle. +class btRaycastVehicle : public btTypedConstraint +{ +public: + class btVehicleTuning + { + public: + + btVehicleTuning() + :m_suspensionStiffness(5.88f), + m_suspensionCompression(0.83f), + m_suspensionDamping(0.88f), + m_maxSuspensionTravelCm(500.f), + m_frictionSlip(10.5f) + { + } + float m_suspensionStiffness; + float m_suspensionCompression; + float m_suspensionDamping; + float m_maxSuspensionTravelCm; + float m_frictionSlip; + + }; +private: + + btScalar m_tau; + btScalar m_damping; + btVehicleRaycaster* m_vehicleRaycaster; + float m_pitchControl; + float m_steeringValue; + float m_currentVehicleSpeedKmHour; + + btRigidBody* m_chassisBody; + + int m_indexRightAxis; + int m_indexUpAxis; + int m_indexForwardAxis; + + void defaultInit(const btVehicleTuning& tuning); + +public: + + //constructor to create a car from an existing rigidbody + btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ); + + virtual ~btRaycastVehicle() ; + + + + + + btScalar rayCast(btWheelInfo& wheel); + + virtual void updateVehicle(btScalar step); + + void resetSuspension(); + + btScalar getSteeringValue(int wheel) const; + + void setSteeringValue(btScalar steering,int wheel); + + + void applyEngineForce(btScalar force, int wheel); + + const btTransform& getWheelTransformWS( int wheelIndex ) const; + + void updateWheelTransform( int wheelIndex ); + + void setRaycastWheelInfo( int wheelIndex , bool isInContact, const btVector3& hitPoint, const btVector3& hitNormal,btScalar depth); + + btWheelInfo& addWheel( const btVector3& connectionPointCS0, const btVector3& wheelDirectionCS0,const btVector3& wheelAxleCS,btScalar suspensionRestLength,btScalar wheelRadius,const btVehicleTuning& tuning, bool isFrontWheel); + + inline int getNumWheels() const { + return m_wheelInfo.size(); + } + + std::vector<btWheelInfo> m_wheelInfo; + + + const btWheelInfo& getWheelInfo(int index) const; + + btWheelInfo& getWheelInfo(int index); + + void updateWheelTransformsWS(btWheelInfo& wheel ); + + + void setBrake(float brake,int wheelIndex); + + void setPitchControl(float pitch) + { + m_pitchControl = pitch; + } + + void updateSuspension(btScalar deltaTime); + + void updateFriction(btScalar timeStep); + + + + inline btRigidBody* getRigidBody() + { + return m_chassisBody; + } + + const btRigidBody* getRigidBody() const + { + return m_chassisBody; + } + + inline int getRightAxis() const + { + return m_indexRightAxis; + } + inline int getUpAxis() const + { + return m_indexUpAxis; + } + + inline int getForwardAxis() const + { + return m_indexForwardAxis; + } + + virtual void setCoordinateSystem(int rightIndex,int upIndex,int forwardIndex) + { + m_indexRightAxis = rightIndex; + m_indexUpAxis = upIndex; + m_indexForwardAxis = forwardIndex; + } + + virtual void buildJacobian() + { + //not yet + } + + virtual void solveConstraint(btScalar timeStep) + { + //not yet + } + + +}; + +#endif //RAYCASTVEHICLE_H + diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h new file mode 100644 index 00000000000..5be3693fe73 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -0,0 +1,35 @@ +/* + * 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. +*/ +#ifndef VEHICLE_RAYCASTER_H +#define VEHICLE_RAYCASTER_H + +#include "LinearMath/btVector3.h" + +/// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting +struct btVehicleRaycaster +{ +virtual ~btVehicleRaycaster() +{ +} + struct btVehicleRaycasterResult + { + btVehicleRaycasterResult() :m_distFraction(-1.f){}; + btVector3 m_hitPointInWorld; + btVector3 m_hitNormalInWorld; + btScalar m_distFraction; + }; + + virtual void* castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result) = 0; + +}; + +#endif //VEHICLE_RAYCASTER_H + diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp new file mode 100644 index 00000000000..43baf56edbc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.cpp @@ -0,0 +1,55 @@ +/* + * 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) +{ + + + 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 >= -0.1f) + { + m_suspensionRelativeVelocity = 0.0f; + m_clippedInvContactDotSuspension = 1.0f / 0.1f; + } + else + { + btScalar inv = -1.f / 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 = 0.0f; + m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; + m_clippedInvContactDotSuspension = 1.0f; + } +} diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h new file mode 100644 index 00000000000..83a3b114ab9 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -0,0 +1,116 @@ +/* + * 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. +*/ +#ifndef WHEEL_INFO_H +#define WHEEL_INFO_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btTransform.h" + +class btRigidBody; + +struct btWheelInfoConstructionInfo +{ + btVector3 m_chassisConnectionCS; + btVector3 m_wheelDirectionCS; + btVector3 m_wheelAxleCS; + btScalar m_suspensionRestLength; + btScalar m_maxSuspensionTravelCm; + btScalar m_wheelRadius; + + float m_suspensionStiffness; + float m_wheelsDampingCompression; + float m_wheelsDampingRelaxation; + float m_frictionSlip; + bool m_bIsFrontWheel; + +}; + +/// btWheelInfo contains information per wheel about friction and suspension. +struct btWheelInfo +{ + struct RaycastInfo + { + //set by raycaster + btVector3 m_contactNormalWS;//contactnormal + btVector3 m_contactPointWS;//raycast hitpoint + btScalar m_suspensionLength; + btVector3 m_hardPointWS;//raycast starting point + btVector3 m_wheelDirectionWS; //direction in worldspace + btVector3 m_wheelAxleWS; // axle in worldspace + bool m_isInContact; + void* m_groundObject; //could be general void* ptr + }; + + RaycastInfo m_raycastInfo; + + btTransform m_worldTransform; + + btVector3 m_chassisConnectionPointCS; //const + btVector3 m_wheelDirectionCS;//const + btVector3 m_wheelAxleCS; // const or modified by steering + btScalar m_suspensionRestLength1;//const + btScalar m_maxSuspensionTravelCm; + btScalar getSuspensionRestLength() const; + btScalar m_wheelsRadius;//const + btScalar m_suspensionStiffness;//const + btScalar m_wheelsDampingCompression;//const + btScalar m_wheelsDampingRelaxation;//const + btScalar m_frictionSlip; + btScalar m_steering; + btScalar m_rotation; + btScalar m_deltaRotation; + btScalar m_rollInfluence; + + btScalar m_engineForce; + + btScalar m_brake; + + bool m_bIsFrontWheel; + + void* m_clientInfo;//can be used to store pointer to sync transforms... + + btWheelInfo(btWheelInfoConstructionInfo& ci) + + { + + m_suspensionRestLength1 = ci.m_suspensionRestLength; + m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm; + + m_wheelsRadius = ci.m_wheelRadius; + m_suspensionStiffness = ci.m_suspensionStiffness; + m_wheelsDampingCompression = ci.m_wheelsDampingCompression; + m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation; + m_chassisConnectionPointCS = ci.m_chassisConnectionCS; + m_wheelDirectionCS = ci.m_wheelDirectionCS; + m_wheelAxleCS = ci.m_wheelAxleCS; + m_frictionSlip = ci.m_frictionSlip; + m_steering = 0.f; + m_engineForce = 0.f; + m_rotation = 0.f; + m_deltaRotation = 0.f; + m_brake = 0.f; + m_rollInfluence = 0.1f; + m_bIsFrontWheel = ci.m_bIsFrontWheel; + + } + + void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo); + + btScalar m_clippedInvContactDotSuspension; + btScalar m_suspensionRelativeVelocity; + //calculated by suspension + btScalar m_wheelsSuspensionForce; + btScalar m_skidInfo; + +}; + +#endif //WHEEL_INFO_H + diff --git a/extern/bullet2/src/CMakeLists.txt b/extern/bullet2/src/CMakeLists.txt new file mode 100644 index 00000000000..0ae1a7ab6ab --- /dev/null +++ b/extern/bullet2/src/CMakeLists.txt @@ -0,0 +1 @@ +SUBDIRS( BulletCollision BulletDynamics LinearMath ) diff --git a/extern/bullet2/src/LinearMath/CMakeLists.txt b/extern/bullet2/src/LinearMath/CMakeLists.txt new file mode 100644 index 00000000000..b9546167534 --- /dev/null +++ b/extern/bullet2/src/LinearMath/CMakeLists.txt @@ -0,0 +1,9 @@ + +INCLUDE_DIRECTORIES( +${BULLET_PHYSICS_SOURCE_DIR}/src } +) + +ADD_LIBRARY(LibLinearMath +btQuickprof.cpp +) + diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h new file mode 100644 index 00000000000..1cc7d4ebdf2 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -0,0 +1,57 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef AABB_UTIL2 +#define AABB_UTIL2 + +#include "LinearMath/btVector3.h" + +#define btMin(a,b) ((a < b ? a : b)) +#define btMax(a,b) ((a > b ? a : b)) + + +/// conservative test for overlap between two aabbs +SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, + const btVector3 &aabbMin2, const btVector3 &aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; +} + +/// conservative test for overlap between triangle and aabb +SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices, + const btVector3 &aabbMin, const btVector3 &aabbMax) +{ + const btVector3 &p1 = vertices[0]; + const btVector3 &p2 = vertices[1]; + const btVector3 &p3 = vertices[2]; + + if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false; + if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false; + + if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false; + if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false; + + if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false; + if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false; + return true; +} + +#endif + diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h new file mode 100644 index 00000000000..c64f1352e10 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -0,0 +1,34 @@ +#ifndef DEFAULT_MOTION_STATE_H +#define DEFAULT_MOTION_STATE_H + +///btDefaultMotionState provides a common implementation to synchronize world transforms with offsets +struct btDefaultMotionState : public btMotionState +{ + btTransform m_graphicsWorldTrans; + btTransform m_centerOfMassOffset; + btTransform m_startWorldTrans; + void* m_userPointer; + + btDefaultMotionState(const btTransform& startTrans,const btTransform& centerOfMassOffset = btTransform::getIdentity()) + : m_graphicsWorldTrans(startTrans), + m_centerOfMassOffset(centerOfMassOffset), + m_startWorldTrans(startTrans), + m_userPointer(0) + + { + } + + ///synchronizes world transform from user to physics + virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) + { + centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans ; + } + + ///synchronizes world transform from physics to user + virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans) + { + m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset ; + } +}; + +#endif //DEFAULT_MOTION_STATE_H
\ No newline at end of file diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h new file mode 100644 index 00000000000..86db735ce94 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -0,0 +1,69 @@ +/* +Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. +*/ + + +#ifndef IDEBUG_DRAW__H +#define IDEBUG_DRAW__H + +#include "LinearMath/btVector3.h" + + +class btIDebugDraw +{ + public: + + enum DebugDrawModes + { + DBG_NoDebug=0, + DBG_DrawWireframe = 1, + DBG_DrawAabb=2, + DBG_DrawFeaturesText=4, + DBG_DrawContactPoints=8, + DBG_NoDeactivation=16, + DBG_NoHelpText = 32, + DBG_DrawText=64, + DBG_ProfileTimings = 128, + DBG_EnableSatComparison = 256, + DBG_DisableBulletLCP = 512, + DBG_EnableCCD = 1024, + DBG_MAX_DEBUG_DRAW_MODE + }; + + virtual ~btIDebugDraw() {}; + + virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0; + + virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,float distance,int lifeTime,const btVector3& color)=0; + + virtual void setDebugMode(int debugMode) =0; + + virtual int getDebugMode() const = 0; + + +}; + +#endif //IDEBUG_DRAW__H + diff --git a/extern/bullet2/src/LinearMath/btList.h b/extern/bullet2/src/LinearMath/btList.h new file mode 100644 index 00000000000..c87b47faf2b --- /dev/null +++ b/extern/bullet2/src/LinearMath/btList.h @@ -0,0 +1,73 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef GEN_LIST_H +#define GEN_LIST_H + +class btGEN_Link { +public: + btGEN_Link() : m_next(0), m_prev(0) {} + btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {} + + btGEN_Link *getNext() const { return m_next; } + btGEN_Link *getPrev() const { return m_prev; } + + bool isHead() const { return m_prev == 0; } + bool isTail() const { return m_next == 0; } + + void insertBefore(btGEN_Link *link) { + m_next = link; + m_prev = link->m_prev; + m_next->m_prev = this; + m_prev->m_next = this; + } + + void insertAfter(btGEN_Link *link) { + m_next = link->m_next; + m_prev = link; + m_next->m_prev = this; + m_prev->m_next = this; + } + + void remove() { + m_next->m_prev = m_prev; + m_prev->m_next = m_next; + } + +private: + btGEN_Link *m_next; + btGEN_Link *m_prev; +}; + +class btGEN_List { +public: + btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {} + + btGEN_Link *getHead() const { return m_head.getNext(); } + btGEN_Link *getTail() const { return m_tail.getPrev(); } + + void addHead(btGEN_Link *link) { link->insertAfter(&m_head); } + void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); } + +private: + btGEN_Link m_head; + btGEN_Link m_tail; +}; + +#endif + + + diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h new file mode 100644 index 00000000000..c3cc90a82c7 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -0,0 +1,395 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef btMatrix3x3_H +#define btMatrix3x3_H + +#include "LinearMath/btScalar.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btQuaternion.h" + + +class btMatrix3x3 { + public: + btMatrix3x3 () {} + +// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); } + + explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); } + /* + template <typename btScalar> + Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + setEulerYPR(yaw, pitch, roll); + } + */ + btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz, + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) + { + setValue(xx, xy, xz, + yx, yy, yz, + zx, zy, zz); + } + + btVector3 getColumn(int i) const + { + return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); + } + + const btVector3& getRow(int i) const + { + return m_el[i]; + } + + + SIMD_FORCE_INLINE btVector3& operator[](int i) + { + assert(0 <= i && i < 3); + return m_el[i]; + } + + const btVector3& operator[](int i) const + { + assert(0 <= i && i < 3); + return m_el[i]; + } + + btMatrix3x3& operator*=(const btMatrix3x3& m); + + + void setFromOpenGLSubMatrix(const btScalar *m) + { + m_el[0][0] = (m[0]); + m_el[1][0] = (m[1]); + m_el[2][0] = (m[2]); + m_el[0][1] = (m[4]); + m_el[1][1] = (m[5]); + m_el[2][1] = (m[6]); + m_el[0][2] = (m[8]); + m_el[1][2] = (m[9]); + m_el[2][2] = (m[10]); + } + + void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, + const btScalar& yx, const btScalar& yy, const btScalar& yz, + const btScalar& zx, const btScalar& zy, const btScalar& zz) + { + m_el[0][0] = btScalar(xx); + m_el[0][1] = btScalar(xy); + m_el[0][2] = btScalar(xz); + m_el[1][0] = btScalar(yx); + m_el[1][1] = btScalar(yy); + m_el[1][2] = btScalar(yz); + m_el[2][0] = btScalar(zx); + m_el[2][1] = btScalar(zy); + m_el[2][2] = btScalar(zz); + } + + void setRotation(const btQuaternion& q) + { + btScalar d = q.length2(); + assert(d != btScalar(0.0)); + btScalar s = btScalar(2.0) / d; + btScalar xs = q[0] * s, ys = q[1] * s, zs = q[2] * s; + btScalar wx = q[3] * xs, wy = q[3] * ys, wz = q[3] * zs; + btScalar xx = q[0] * xs, xy = q[0] * ys, xz = q[0] * zs; + btScalar yy = q[1] * ys, yz = q[1] * zs, zz = q[2] * zs; + setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy, + xy + wz, btScalar(1.0) - (xx + zz), yz - wx, + xz - wy, yz + wx, btScalar(1.0) - (xx + yy)); + } + + + + void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + + btScalar cy(btCos(yaw)); + btScalar sy(btSin(yaw)); + btScalar cp(btCos(pitch)); + btScalar sp(btSin(pitch)); + btScalar cr(btCos(roll)); + btScalar sr(btSin(roll)); + btScalar cc = cy * cr; + btScalar cs = cy * sr; + btScalar sc = sy * cr; + btScalar ss = sy * sr; + setValue(cc - sp * ss, -cs - sp * sc, -sy * cp, + cp * sr, cp * cr, -sp, + sc + sp * cs, -ss + sp * cc, cy * cp); + + } + + /** + * setEulerZYX + * @param euler a const reference to a btVector3 of euler angles + * These angles are used to produce a rotation matrix. The euler + * angles are applied in ZYX order. I.e a vector is first rotated + * about X then Y and then Z + **/ + + void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { + btScalar ci ( btCos(eulerX)); + btScalar cj ( btCos(eulerY)); + btScalar ch ( btCos(eulerZ)); + btScalar si ( btSin(eulerX)); + btScalar sj ( btSin(eulerY)); + btScalar sh ( btSin(eulerZ)); + btScalar cc = ci * ch; + btScalar cs = ci * sh; + btScalar sc = si * ch; + btScalar ss = si * sh; + + setValue(cj * ch, sj * sc - cs, sj * cc + ss, + cj * sh, sj * ss + cc, sj * cs - sc, + -sj, cj * si, cj * ci); + } + + void setIdentity() + { + setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), + btScalar(0.0), btScalar(1.0), btScalar(0.0), + btScalar(0.0), btScalar(0.0), btScalar(1.0)); + } + + void getOpenGLSubMatrix(btScalar *m) const + { + m[0] = btScalar(m_el[0][0]); + m[1] = btScalar(m_el[1][0]); + m[2] = btScalar(m_el[2][0]); + m[3] = btScalar(0.0); + m[4] = btScalar(m_el[0][1]); + m[5] = btScalar(m_el[1][1]); + m[6] = btScalar(m_el[2][1]); + m[7] = btScalar(0.0); + m[8] = btScalar(m_el[0][2]); + m[9] = btScalar(m_el[1][2]); + m[10] = btScalar(m_el[2][2]); + m[11] = btScalar(0.0); + } + + void getRotation(btQuaternion& q) const + { + btScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2]; + + if (trace > btScalar(0.0)) + { + btScalar s = btSqrt(trace + btScalar(1.0)); + q[3] = s * btScalar(0.5); + s = btScalar(0.5) / s; + + q[0] = (m_el[2][1] - m_el[1][2]) * s; + q[1] = (m_el[0][2] - m_el[2][0]) * s; + q[2] = (m_el[1][0] - m_el[0][1]) * s; + } + else + { + int i = m_el[0][0] < m_el[1][1] ? + (m_el[1][1] < m_el[2][2] ? 2 : 1) : + (m_el[0][0] < m_el[2][2] ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0)); + q[i] = s * btScalar(0.5); + s = btScalar(0.5) / s; + + q[3] = (m_el[k][j] - m_el[j][k]) * s; + q[j] = (m_el[j][i] + m_el[i][j]) * s; + q[k] = (m_el[k][i] + m_el[i][k]) * s; + } + } + + + + void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const + { + pitch = btScalar(btAsin(-m_el[2][0])); + if (pitch < SIMD_2_PI) + { + if (pitch > SIMD_2_PI) + { + yaw = btScalar(btAtan2(m_el[1][0], m_el[0][0])); + roll = btScalar(btAtan2(m_el[2][1], m_el[2][2])); + } + else + { + yaw = btScalar(-btAtan2(-m_el[0][1], m_el[0][2])); + roll = btScalar(0.0); + } + } + else + { + yaw = btScalar(btAtan2(-m_el[0][1], m_el[0][2])); + roll = btScalar(0.0); + } + } + + btVector3 getScaling() const + { + return btVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0], + m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1], + m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]); + } + + + btMatrix3x3 scaled(const btVector3& s) const + { + return btMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2], + m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2], + m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]); + } + + btScalar determinant() const; + btMatrix3x3 adjoint() const; + btMatrix3x3 absolute() const; + btMatrix3x3 transpose() const; + btMatrix3x3 inverse() const; + + btMatrix3x3 transposeTimes(const btMatrix3x3& m) const; + btMatrix3x3 timesTranspose(const btMatrix3x3& m) const; + + btScalar tdot(int c, const btVector3& v) const + { + return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2]; + } + + protected: + btScalar cofac(int r1, int c1, int r2, int c2) const + { + return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1]; + } + + btVector3 m_el[3]; + }; + + SIMD_FORCE_INLINE btMatrix3x3& + btMatrix3x3::operator*=(const btMatrix3x3& m) + { + setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]), + m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]), + m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2])); + return *this; + } + + SIMD_FORCE_INLINE btScalar + btMatrix3x3::determinant() const + { + return triple((*this)[0], (*this)[1], (*this)[2]); + } + + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::absolute() const + { + return btMatrix3x3( + btFabs(m_el[0][0]), btFabs(m_el[0][1]), btFabs(m_el[0][2]), + btFabs(m_el[1][0]), btFabs(m_el[1][1]), btFabs(m_el[1][2]), + btFabs(m_el[2][0]), btFabs(m_el[2][1]), btFabs(m_el[2][2])); + } + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::transpose() const + { + return btMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0], + m_el[0][1], m_el[1][1], m_el[2][1], + m_el[0][2], m_el[1][2], m_el[2][2]); + } + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::adjoint() const + { + return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2), + cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0), + cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1)); + } + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::inverse() const + { + btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1)); + btScalar det = (*this)[0].dot(co); + assert(det != btScalar(0.0f)); + btScalar s = btScalar(1.0f) / det; + return btMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, + co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, + co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); + } + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::transposeTimes(const btMatrix3x3& m) const + { + return btMatrix3x3( + m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0], + m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1], + m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2], + m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0], + m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1], + m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2], + m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0], + m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1], + m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]); + } + + SIMD_FORCE_INLINE btMatrix3x3 + btMatrix3x3::timesTranspose(const btMatrix3x3& m) const + { + return btMatrix3x3( + m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]), + m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]), + m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2])); + + } + + SIMD_FORCE_INLINE btVector3 + operator*(const btMatrix3x3& m, const btVector3& v) + { + return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v)); + } + + + SIMD_FORCE_INLINE btVector3 + operator*(const btVector3& v, const btMatrix3x3& m) + { + return btVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v)); + } + + SIMD_FORCE_INLINE btMatrix3x3 + operator*(const btMatrix3x3& m1, const btMatrix3x3& m2) + { + return btMatrix3x3( + m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]), + m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]), + m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2])); + } + + + SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) { + return btMatrix3x3( + m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0], + m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1], + m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2], + m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0], + m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1], + m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2], + m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0], + m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1], + m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]); +} + + +#endif diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet2/src/LinearMath/btMinMax.h new file mode 100644 index 00000000000..1b8a3633f38 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btMinMax.h @@ -0,0 +1,69 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef GEN_MINMAX_H +#define GEN_MINMAX_H + +template <class T> +SIMD_FORCE_INLINE const T& GEN_min(const T& a, const T& b) +{ + return b < a ? b : a; +} + +template <class T> +SIMD_FORCE_INLINE const T& GEN_max(const T& a, const T& b) +{ + return a < b ? b : a; +} + +template <class T> +SIMD_FORCE_INLINE const T& GEN_clamped(const T& a, const T& lb, const T& ub) +{ + return a < lb ? lb : (ub < a ? ub : a); +} + +template <class T> +SIMD_FORCE_INLINE void GEN_set_min(T& a, const T& b) +{ + if (b < a) + { + a = b; + } +} + +template <class T> +SIMD_FORCE_INLINE void GEN_set_max(T& a, const T& b) +{ + if (a < b) + { + a = b; + } +} + +template <class T> +SIMD_FORCE_INLINE void GEN_clamp(T& a, const T& lb, const T& ub) +{ + if (a < lb) + { + a = lb; + } + else if (ub < a) + { + a = ub; + } +} + +#endif diff --git a/extern/bullet2/src/LinearMath/btMotionState.h b/extern/bullet2/src/LinearMath/btMotionState.h new file mode 100644 index 00000000000..4bbb3d44888 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btMotionState.h @@ -0,0 +1,38 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_MOTIONSTATE_H +#define BT_MOTIONSTATE_H + +#include "LinearMath/btTransform.h" + +///btMotionState allows the dynamics world to synchronize the updated world transforms with graphics +///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation) +class btMotionState +{ + public: + + virtual ~btMotionState() + { + + } + + virtual void getWorldTransform(btTransform& worldTrans )=0; + + virtual void setWorldTransform(const btTransform& worldTrans)=0; + +}; + +#endif //BT_MOTIONSTATE_H diff --git a/extern/bullet2/src/LinearMath/btPoint3.h b/extern/bullet2/src/LinearMath/btPoint3.h new file mode 100644 index 00000000000..4be7e9015bb --- /dev/null +++ b/extern/bullet2/src/LinearMath/btPoint3.h @@ -0,0 +1,24 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef btPoint3_H +#define btPoint3_H + +#include "LinearMath/btVector3.h" + +typedef btVector3 btPoint3; + +#endif diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h new file mode 100644 index 00000000000..28bb2051dea --- /dev/null +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -0,0 +1,134 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef SIMD_QUADWORD_H +#define SIMD_QUADWORD_H + +#include "LinearMath/btScalar.h" + + + + + +class btQuadWord +{ + protected: + ATTRIBUTE_ALIGNED16 (btScalar m_x); + btScalar m_y; + btScalar m_z; + btScalar m_unusedW; + + public: + + SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } + SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_x)[i]; } + + SIMD_FORCE_INLINE const btScalar& getX() const { return m_x; } + + SIMD_FORCE_INLINE const btScalar& getY() const { return m_y; } + + SIMD_FORCE_INLINE const btScalar& getZ() const { return m_z; } + + SIMD_FORCE_INLINE void setX(float x) { m_x = x;}; + + SIMD_FORCE_INLINE void setY(float y) { m_y = y;}; + + SIMD_FORCE_INLINE void setZ(float z) { m_z = z;}; + + SIMD_FORCE_INLINE const btScalar& x() const { return m_x; } + + SIMD_FORCE_INLINE const btScalar& y() const { return m_y; } + + SIMD_FORCE_INLINE const btScalar& z() const { return m_z; } + + + operator btScalar *() { return &m_x; } + operator const btScalar *() const { return &m_x; } + + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_x=x; + m_y=y; + m_z=z; + } + +/* void getValue(btScalar *m) const + { + m[0] = m_x; + m[1] = m_y; + m[2] = m_z; + } +*/ + SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + { + m_x=x; + m_y=y; + m_z=z; + m_unusedW=w; + } + + SIMD_FORCE_INLINE btQuadWord() : + m_x(0.f),m_y(0.f),m_z(0.f),m_unusedW(0.f) + { + } + + SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) + :m_x(x),m_y(y),m_z(z) + //todo, remove this in release/simd ? + ,m_unusedW(0.f) + { + } + + SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + :m_x(x),m_y(y),m_z(z),m_unusedW(w) + { + } + + + SIMD_FORCE_INLINE void setMax(const btQuadWord& other) + { + if (other.m_x > m_x) + m_x = other.m_x; + + if (other.m_y > m_y) + m_y = other.m_y; + + if (other.m_z > m_z) + m_z = other.m_z; + + if (other.m_unusedW > m_unusedW) + m_unusedW = other.m_unusedW; + } + + SIMD_FORCE_INLINE void setMin(const btQuadWord& other) + { + if (other.m_x < m_x) + m_x = other.m_x; + + if (other.m_y < m_y) + m_y = other.m_y; + + if (other.m_z < m_z) + m_z = other.m_z; + + if (other.m_unusedW < m_unusedW) + m_unusedW = other.m_unusedW; + } + + + +}; + +#endif //SIMD_QUADWORD_H diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h new file mode 100644 index 00000000000..aec25a54955 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -0,0 +1,290 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef SIMD__QUATERNION_H_ +#define SIMD__QUATERNION_H_ + +#include "LinearMath/btVector3.h" + +class btQuaternion : public btQuadWord { +public: + btQuaternion() {} + + // template <typename btScalar> + // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {} + + btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) + : btQuadWord(x, y, z, w) + {} + + btQuaternion(const btVector3& axis, const btScalar& angle) + { + setRotation(axis, angle); + } + + btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + setEuler(yaw, pitch, roll); + } + + void setRotation(const btVector3& axis, const btScalar& angle) + { + btScalar d = axis.length(); + assert(d != btScalar(0.0)); + btScalar s = btSin(angle * btScalar(0.5)) / d; + setValue(axis.x() * s, axis.y() * s, axis.z() * s, + btCos(angle * btScalar(0.5))); + } + + void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + { + btScalar halfYaw = btScalar(yaw) * btScalar(0.5); + btScalar halfPitch = btScalar(pitch) * btScalar(0.5); + btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar cosYaw = btCos(halfYaw); + btScalar sinYaw = btSin(halfYaw); + btScalar cosPitch = btCos(halfPitch); + btScalar sinPitch = btSin(halfPitch); + btScalar cosRoll = btCos(halfRoll); + btScalar sinRoll = btSin(halfRoll); + setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, + cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, + sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, + cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); + } + + btQuaternion& operator+=(const btQuaternion& q) + { + m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3]; + return *this; + } + + btQuaternion& operator-=(const btQuaternion& q) + { + m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3]; + return *this; + } + + btQuaternion& operator*=(const btScalar& s) + { + m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s; + return *this; + } + + + btQuaternion& operator*=(const btQuaternion& q) + { + setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(), + m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(), + m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(), + m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z()); + return *this; + } + + btScalar dot(const btQuaternion& q) const + { + return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3]; + } + + btScalar length2() const + { + return dot(*this); + } + + btScalar length() const + { + return btSqrt(length2()); + } + + btQuaternion& normalize() + { + return *this /= length(); + } + + SIMD_FORCE_INLINE btQuaternion + operator*(const btScalar& s) const + { + return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); + } + + + + btQuaternion operator/(const btScalar& s) const + { + assert(s != btScalar(0.0)); + return *this * (btScalar(1.0) / s); + } + + + btQuaternion& operator/=(const btScalar& s) + { + assert(s != btScalar(0.0)); + return *this *= btScalar(1.0) / s; + } + + + btQuaternion normalized() const + { + return *this / length(); + } + + btScalar angle(const btQuaternion& q) const + { + btScalar s = btSqrt(length2() * q.length2()); + assert(s != btScalar(0.0)); + return btAcos(dot(q) / s); + } + + btScalar getAngle() const + { + btScalar s = 2.f * btAcos(m_unusedW); + return s; + } + + + + btQuaternion inverse() const + { + return btQuaternion(m_x, m_y, m_z, -m_unusedW); + } + + SIMD_FORCE_INLINE btQuaternion + operator+(const btQuaternion& q2) const + { + const btQuaternion& q1 = *this; + return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]); + } + + SIMD_FORCE_INLINE btQuaternion + operator-(const btQuaternion& q2) const + { + const btQuaternion& q1 = *this; + return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]); + } + + SIMD_FORCE_INLINE btQuaternion operator-() const + { + const btQuaternion& q2 = *this; + return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2[3]); + } + + SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const + { + btQuaternion diff,sum; + diff = *this - qd; + sum = *this + qd; + if( diff.dot(diff) > sum.dot(sum) ) + return qd; + return (-qd); + } + + btQuaternion slerp(const btQuaternion& q, const btScalar& t) const + { + btScalar theta = angle(q); + if (theta != btScalar(0.0)) + { + btScalar d = btScalar(1.0) / btSin(theta); + btScalar s0 = btSin((btScalar(1.0) - t) * theta); + btScalar s1 = btSin(t * theta); + return btQuaternion((m_x * s0 + q.x() * s1) * d, + (m_y * s0 + q.y() * s1) * d, + (m_z * s0 + q.z() * s1) * d, + (m_unusedW * s0 + q[3] * s1) * d); + } + else + { + return *this; + } + } + + SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; } + +}; + + + +SIMD_FORCE_INLINE btQuaternion +operator-(const btQuaternion& q) +{ + return btQuaternion(-q.x(), -q.y(), -q.z(), -q[3]); +} + + + + +SIMD_FORCE_INLINE btQuaternion +operator*(const btQuaternion& q1, const btQuaternion& q2) { + return btQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(), + q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(), + q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(), + q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); +} + +SIMD_FORCE_INLINE btQuaternion +operator*(const btQuaternion& q, const btVector3& w) +{ + return btQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(), + q[3] * w.y() + q.z() * w.x() - q.x() * w.z(), + q[3] * w.z() + q.x() * w.y() - q.y() * w.x(), + -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); +} + +SIMD_FORCE_INLINE btQuaternion +operator*(const btVector3& w, const btQuaternion& q) +{ + return btQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(), + w.y() * q[3] + w.z() * q.x() - w.x() * q.z(), + w.z() * q[3] + w.x() * q.y() - w.y() * q.x(), + -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); +} + +SIMD_FORCE_INLINE btScalar +dot(const btQuaternion& q1, const btQuaternion& q2) +{ + return q1.dot(q2); +} + + +SIMD_FORCE_INLINE btScalar +length(const btQuaternion& q) +{ + return q.length(); +} + +SIMD_FORCE_INLINE btScalar +angle(const btQuaternion& q1, const btQuaternion& q2) +{ + return q1.angle(q2); +} + + +SIMD_FORCE_INLINE btQuaternion +inverse(const btQuaternion& q) +{ + return q.inverse(); +} + +SIMD_FORCE_INLINE btQuaternion +slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) +{ + return q1.slerp(q2, t); +} + + +#endif + + + diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp new file mode 100644 index 00000000000..23b8b8b2c40 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -0,0 +1,45 @@ +/************************************************************************ +* QuickProf * +* Copyright (C) 2006 * +* Tyler Streeter tylerstreeter@gmail.com * +* All rights reserved. * +* Web: http://quickprof.sourceforge.net * +* * +* This library is free software; you can redistribute it and/or * +* modify it under the terms of EITHER: * +* (1) The GNU Lesser bteral Public License as published by the Free * +* Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. The text of the GNU Lesser * +* bteral Public License is included with this library in the * +* file license-LGPL.txt. * +* (2) The BSD-style license that is included with this library in * +* the file license-BSD.txt. * +* * +* This library is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * +* license-LGPL.txt and license-BSD.txt for more details. * +************************************************************************/ + +// Please visit the project website (http://quickprof.sourceforge.net) +// for usage instructions. + +// Credits: The Clock class was inspired by the Timer classes in +// Ogre (www.ogre3d.org). + +#include "LinearMath/btQuickprof.h" + +#ifdef USE_QUICKPROF + +// Note: We must declare these private static variables again here to +// avoid link errors. +bool btProfiler::mEnabled = false; +hidden::Clock btProfiler::mClock; +unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0; +unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0; +std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks; +std::ofstream btProfiler::mOutputFile; +bool btProfiler::mFirstFileOutput = true; +btProfiler::BlockTimingMethod btProfiler::mFileOutputMethod; +unsigned long int btProfiler::mCycleNumber = 0; +#endif //USE_QUICKPROF diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h new file mode 100644 index 00000000000..801ef07a946 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -0,0 +1,687 @@ +/************************************************************************ +* QuickProf * +* Copyright (C) 2006 * +* Tyler Streeter tylerstreeter@gmail.com * +* All rights reserved. * +* Web: http://quickprof.sourceforge.net * +* * +* This library is free software; you can redistribute it and/or * +* modify it under the terms of EITHER: * +* (1) The GNU Lesser bteral Public License as published by the Free * +* Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. The text of the GNU Lesser * +* bteral Public License is included with this library in the * +* file license-LGPL.txt. * +* (2) The BSD-style license that is included with this library in * +* the file license-BSD.txt. * +* * +* This library is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * +* license-LGPL.txt and license-BSD.txt for more details. * +************************************************************************/ + +// Please visit the project website (http://quickprof.sourceforge.net) +// for usage instructions. + +// Credits: The Clock class was inspired by the Timer classes in +// Ogre (www.ogre3d.org). + +#ifndef QUICK_PROF_H +#define QUICK_PROF_H + +#define USE_QUICKPROF 1 + +#ifdef USE_QUICKPROF + + +#include <iostream> +#include <fstream> +#include <string> +#include <map> + +#ifdef __PPU__ +#include <sys/sys_time.h> +#include <stdio.h> +typedef uint64_t __int64; +#endif + +#if defined(WIN32) || defined(_WIN32) + #define USE_WINDOWS_TIMERS + #include <windows.h> + #include <time.h> +#else + #include <sys/time.h> +#endif + +#define mymin(a,b) (a > b ? a : b) +namespace hidden +{ + /// A simple data structure representing a single timed block + /// of code. + struct ProfileBlock + { + ProfileBlock() + { + currentBlockStartMicroseconds = 0; + currentCycleTotalMicroseconds = 0; + lastCycleTotalMicroseconds = 0; + totalMicroseconds = 0; + } + + /// The starting time (in us) of the current block update. + unsigned long int currentBlockStartMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// current profiling cycle. + unsigned long int currentCycleTotalMicroseconds; + + /// The accumulated time (in us) spent in this block during the + /// past profiling cycle. + unsigned long int lastCycleTotalMicroseconds; + + /// The total accumulated time (in us) spent in this block. + unsigned long int totalMicroseconds; + }; + + class Clock + { + public: + Clock() + { +#ifdef USE_WINDOWS_TIMERS + QueryPerformanceFrequency(&mClockFrequency); +#endif + reset(); + } + + ~Clock() + { + } + + /// Resets the initial reference time. + void reset() + { +#ifdef USE_WINDOWS_TIMERS + QueryPerformanceCounter(&mStartTime); + mStartTick = GetTickCount(); + mPrevElapsedTime = 0; +#else +#ifdef __PPU__ + + typedef uint64_t __int64; + typedef __int64 ClockSize; + ClockSize newTime; + __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + mStartTime = newTime; +#else + gettimeofday(&mStartTime, NULL); +#endif + +#endif + } + + /// Returns the time in ms since the last call to reset or since + /// the Clock was created. + unsigned long int getTimeMilliseconds() + { +#ifdef USE_WINDOWS_TIMERS + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + mStartTime.QuadPart; + + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + mClockFrequency.QuadPart); + + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + mClockFrequency.QuadPart / 1000, elapsedTime - + mPrevElapsedTime); + mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + + // Recompute the number of millisecond ticks elapsed. + msecTicks = (unsigned long)(1000 * elapsedTime / + mClockFrequency.QuadPart); + } + + // Store the current elapsed time for adjustments next time. + mPrevElapsedTime = elapsedTime; + + return msecTicks; +#else + +#ifdef __PPU__ + __int64 freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq) / 1000.0; + typedef uint64_t __int64; + typedef __int64 ClockSize; + ClockSize newTime; + __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + + return (newTime-mStartTime) / dFreq; +#else + + struct timeval currentTime; + gettimeofday(¤tTime, NULL); + return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + + (currentTime.tv_usec - mStartTime.tv_usec) / 1000; +#endif //__PPU__ +#endif + } + + /// Returns the time in us since the last call to reset or since + /// the Clock was created. + unsigned long int getTimeMicroseconds() + { +#ifdef USE_WINDOWS_TIMERS + LARGE_INTEGER currentTime; + QueryPerformanceCounter(¤tTime); + LONGLONG elapsedTime = currentTime.QuadPart - + mStartTime.QuadPart; + + // Compute the number of millisecond ticks elapsed. + unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / + mClockFrequency.QuadPart); + + // Check for unexpected leaps in the Win32 performance counter. + // (This is caused by unexpected data across the PCI to ISA + // bridge, aka south bridge. See Microsoft KB274323.) + unsigned long elapsedTicks = GetTickCount() - mStartTick; + signed long msecOff = (signed long)(msecTicks - elapsedTicks); + if (msecOff < -100 || msecOff > 100) + { + // Adjust the starting time forwards. + LONGLONG msecAdjustment = mymin(msecOff * + mClockFrequency.QuadPart / 1000, elapsedTime - + mPrevElapsedTime); + mStartTime.QuadPart += msecAdjustment; + elapsedTime -= msecAdjustment; + } + + // Store the current elapsed time for adjustments next time. + mPrevElapsedTime = elapsedTime; + + // Convert to microseconds. + unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / + mClockFrequency.QuadPart); + + return usecTicks; +#else + +#ifdef __PPU__ + __int64 freq=sys_time_get_timebase_frequency(); + double dFreq=((double) freq)/ 1000000.0; + typedef uint64_t __int64; + typedef __int64 ClockSize; + ClockSize newTime; + __asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory"); + + return (newTime-mStartTime) / dFreq; +#else + + struct timeval currentTime; + gettimeofday(¤tTime, NULL); + return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + + (currentTime.tv_usec - mStartTime.tv_usec); +#endif//__PPU__ +#endif + } + + private: +#ifdef USE_WINDOWS_TIMERS + LARGE_INTEGER mClockFrequency; + DWORD mStartTick; + LONGLONG mPrevElapsedTime; + LARGE_INTEGER mStartTime; +#else +#ifdef __PPU__ + uint64_t mStartTime; +#else + struct timeval mStartTime; +#endif +#endif //__PPU__ + + }; +}; + +/// A static class that manages timing for a set of profiling blocks. +class btProfiler +{ +public: + /// A set of ways to retrieve block timing data. + enum BlockTimingMethod + { + /// The total time spent in the block (in seconds) since the + /// profiler was initialized. + BLOCK_TOTAL_SECONDS, + + /// The total time spent in the block (in ms) since the + /// profiler was initialized. + BLOCK_TOTAL_MILLISECONDS, + + /// The total time spent in the block (in us) since the + /// profiler was initialized. + BLOCK_TOTAL_MICROSECONDS, + + /// The total time spent in the block, as a % of the total + /// elapsed time since the profiler was initialized. + BLOCK_TOTAL_PERCENT, + + /// The time spent in the block (in seconds) in the most recent + /// profiling cycle. + BLOCK_CYCLE_SECONDS, + + /// The time spent in the block (in ms) in the most recent + /// profiling cycle. + BLOCK_CYCLE_MILLISECONDS, + + /// The time spent in the block (in us) in the most recent + /// profiling cycle. + BLOCK_CYCLE_MICROSECONDS, + + /// The time spent in the block (in seconds) in the most recent + /// profiling cycle, as a % of the total cycle time. + BLOCK_CYCLE_PERCENT + }; + + /// Initializes the profiler. This must be called first. If this is + /// never called, the profiler is effectively disabled; all other + /// functions will return immediately. The first parameter + /// is the name of an output data file; if this string is not empty, + /// data will be saved on every profiling cycle; if this string is + /// empty, no data will be saved to a file. The second parameter + /// determines which timing method is used when printing data to the + /// output file. + inline static void init(const std::string outputFilename="", + BlockTimingMethod outputMethod=BLOCK_CYCLE_MILLISECONDS); + + /// Cleans up allocated memory. + inline static void destroy(); + + /// Begins timing the named block of code. + inline static void beginBlock(const std::string& name); + + /// Updates the accumulated time spent in the named block by adding + /// the elapsed time since the last call to startBlock for this block + /// name. + inline static void endBlock(const std::string& name); + + /// Returns the time spent in the named block according to the + /// given timing method. See comments on BlockTimingMethod for details. + inline static double getBlockTime(const std::string& name, + BlockTimingMethod method=BLOCK_CYCLE_MILLISECONDS); + + /// Defines the end of a profiling cycle. Use this regularly if you + /// want to generate detailed timing information. This must not be + /// called within a timing block. + inline static void endProfilingCycle(); + + /// A helper function that creates a string of statistics for + /// each timing block. This is mainly for printing an overall + /// summary to the command line. + inline static std::string createStatsString( + BlockTimingMethod method=BLOCK_TOTAL_PERCENT); + +//private: + inline btProfiler(); + + inline ~btProfiler(); + + /// Prints an error message to standard output. + inline static void printError(const std::string& msg) + { + std::cout << "[QuickProf error] " << msg << std::endl; + } + + /// Determines whether the profiler is enabled. + static bool mEnabled; + + /// The clock used to time profile blocks. + static hidden::Clock mClock; + + /// The starting time (in us) of the current profiling cycle. + static unsigned long int mCurrentCycleStartMicroseconds; + + /// The duration (in us) of the most recent profiling cycle. + static unsigned long int mLastCycleDurationMicroseconds; + + /// Internal map of named profile blocks. + static std::map<std::string, hidden::ProfileBlock*> mProfileBlocks; + + /// The data file used if this feature is enabled in 'init.' + static std::ofstream mOutputFile; + + /// Tracks whether we have begun print data to the output file. + static bool mFirstFileOutput; + + /// The method used when printing timing data to an output file. + static BlockTimingMethod mFileOutputMethod; + + /// The number of the current profiling cycle. + static unsigned long int mCycleNumber; +}; + + +btProfiler::btProfiler() +{ + // This never gets called because a btProfiler instance is never + // created. +} + +btProfiler::~btProfiler() +{ + // This never gets called because a btProfiler instance is never + // created. +} + +void btProfiler::init(const std::string outputFilename, + BlockTimingMethod outputMethod) +{ + mEnabled = true; + + if (!outputFilename.empty()) + { + mOutputFile.open(outputFilename.c_str()); + } + + mFileOutputMethod = outputMethod; + + mClock.reset(); + + // Set the start time for the first cycle. + mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); +} + +void btProfiler::destroy() +{ + if (!mEnabled) + { + return; + } + + if (mOutputFile.is_open()) + { + mOutputFile.close(); + } + + // Destroy all ProfileBlocks. + while (!mProfileBlocks.empty()) + { + delete (*mProfileBlocks.begin()).second; + mProfileBlocks.erase(mProfileBlocks.begin()); + } +} + +void btProfiler::beginBlock(const std::string& name) +{ + if (!mEnabled) + { + return; + } + + if (name.empty()) + { + printError("Cannot allow unnamed profile blocks."); + return; + } + + hidden::ProfileBlock* block = mProfileBlocks[name]; + + if (!block) + { + // Create a new ProfileBlock. + mProfileBlocks[name] = new hidden::ProfileBlock(); + block = mProfileBlocks[name]; + } + + // We do this at the end to get more accurate results. + block->currentBlockStartMicroseconds = mClock.getTimeMicroseconds(); +} + +void btProfiler::endBlock(const std::string& name) +{ + if (!mEnabled) + { + return; + } + + // We do this at the beginning to get more accurate results. + unsigned long int endTick = mClock.getTimeMicroseconds(); + + hidden::ProfileBlock* block = mProfileBlocks[name]; + + if (!block) + { + // The named block does not exist. Print an error. + printError("The profile block named '" + name + + "' does not exist."); + return; + } + + unsigned long int blockDuration = endTick - + block->currentBlockStartMicroseconds; + block->currentCycleTotalMicroseconds += blockDuration; + block->totalMicroseconds += blockDuration; +} + +double btProfiler::getBlockTime(const std::string& name, + BlockTimingMethod method) +{ + if (!mEnabled) + { + return 0; + } + + hidden::ProfileBlock* block = mProfileBlocks[name]; + + if (!block) + { + // The named block does not exist. Print an error. + printError("The profile block named '" + name + + "' does not exist."); + return 0; + } + + double result = 0; + + switch(method) + { + case BLOCK_TOTAL_SECONDS: + result = (double)block->totalMicroseconds * (double)0.000001; + break; + case BLOCK_TOTAL_MILLISECONDS: + result = (double)block->totalMicroseconds * (double)0.001; + break; + case BLOCK_TOTAL_MICROSECONDS: + result = (double)block->totalMicroseconds; + break; + case BLOCK_TOTAL_PERCENT: + { + double timeSinceInit = (double)mClock.getTimeMicroseconds(); + if (timeSinceInit <= 0) + { + result = 0; + } + else + { + result = 100.0 * (double)block->totalMicroseconds / + timeSinceInit; + } + break; + } + case BLOCK_CYCLE_SECONDS: + result = (double)block->lastCycleTotalMicroseconds * + (double)0.000001; + break; + case BLOCK_CYCLE_MILLISECONDS: + result = (double)block->lastCycleTotalMicroseconds * + (double)0.001; + break; + case BLOCK_CYCLE_MICROSECONDS: + result = (double)block->lastCycleTotalMicroseconds; + break; + case BLOCK_CYCLE_PERCENT: + { + if (0 == mLastCycleDurationMicroseconds) + { + // We have not yet finished a cycle, so just return zero + // percent to avoid a divide by zero error. + result = 0; + } + else + { + result = 100.0 * (double)block->lastCycleTotalMicroseconds / + mLastCycleDurationMicroseconds; + } + break; + } + default: + break; + } + + return result; +} + +void btProfiler::endProfilingCycle() +{ + if (!mEnabled) + { + return; + } + + // Store the duration of the cycle that just finished. + mLastCycleDurationMicroseconds = mClock.getTimeMicroseconds() - + mCurrentCycleStartMicroseconds; + + // For each block, update data for the cycle that just finished. + std::map<std::string, hidden::ProfileBlock*>::iterator iter; + for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) + { + hidden::ProfileBlock* block = (*iter).second; + block->lastCycleTotalMicroseconds = + block->currentCycleTotalMicroseconds; + block->currentCycleTotalMicroseconds = 0; + } + + if (mOutputFile.is_open()) + { + // Print data to the output file. + if (mFirstFileOutput) + { + // On the first iteration, print a header line that shows the + // names of each profiling block. + mOutputFile << "#cycle, "; + + for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); + ++iter) + { + mOutputFile << (*iter).first << ", "; + } + + mOutputFile << std::endl; + mFirstFileOutput = false; + } + + mOutputFile << mCycleNumber << ", "; + + for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); + ++iter) + { + mOutputFile << getBlockTime((*iter).first, mFileOutputMethod) + << ", "; + } + + mOutputFile << std::endl; + } + + ++mCycleNumber; + mCurrentCycleStartMicroseconds = mClock.getTimeMicroseconds(); +} + +std::string btProfiler::createStatsString(BlockTimingMethod method) +{ + if (!mEnabled) + { + return ""; + } + + std::string s; + std::string suffix; + + switch(method) + { + case BLOCK_TOTAL_SECONDS: + suffix = "s"; + break; + case BLOCK_TOTAL_MILLISECONDS: + suffix = "ms"; + break; + case BLOCK_TOTAL_MICROSECONDS: + suffix = "us"; + break; + case BLOCK_TOTAL_PERCENT: + { + suffix = "%"; + break; + } + case BLOCK_CYCLE_SECONDS: + suffix = "s"; + break; + case BLOCK_CYCLE_MILLISECONDS: + suffix = "ms"; + break; + case BLOCK_CYCLE_MICROSECONDS: + suffix = "us"; + break; + case BLOCK_CYCLE_PERCENT: + { + suffix = "%"; + break; + } + default: + break; + } + + std::map<std::string, hidden::ProfileBlock*>::iterator iter; + for (iter = mProfileBlocks.begin(); iter != mProfileBlocks.end(); ++iter) + { + if (iter != mProfileBlocks.begin()) + { + s += "\n"; + } + + char blockTime[64]; + sprintf(blockTime, "%lf", getBlockTime((*iter).first, method)); + + s += (*iter).first; + s += ": "; + s += blockTime; + s += " "; + s += suffix; + } + + return s; +} + + +#define BEGIN_PROFILE(a) btProfiler::beginBlock(a) +#define END_PROFILE(a) btProfiler::endBlock(a) + +#else //USE_QUICKPROF +#define BEGIN_PROFILE(a) +#define END_PROFILE(a) + +#endif //USE_QUICKPROF + +#endif //QUICK_PROF_H + diff --git a/extern/bullet2/src/LinearMath/btRandom.h b/extern/bullet2/src/LinearMath/btRandom.h new file mode 100644 index 00000000000..fdf65e01caf --- /dev/null +++ b/extern/bullet2/src/LinearMath/btRandom.h @@ -0,0 +1,42 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef GEN_RANDOM_H +#define GEN_RANDOM_H + +#ifdef MT19937 + +#include <limits.h> +#include <mt19937.h> + +#define GEN_RAND_MAX UINT_MAX + +SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); } +SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); } + +#else + +#include <stdlib.h> + +#define GEN_RAND_MAX RAND_MAX + +SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); } +SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); } + +#endif + +#endif + diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h new file mode 100644 index 00000000000..dea040a80bd --- /dev/null +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -0,0 +1,126 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef SIMD___SCALAR_H +#define SIMD___SCALAR_H + +#include <math.h> + +#include <cstdlib> +#include <cfloat> +#include <float.h> + +#ifdef WIN32 + + #if defined(__MINGW32__) || defined(__CYGWIN__) + #define SIMD_FORCE_INLINE inline + #else + #pragma warning(disable:4530) + #pragma warning(disable:4996) + #pragma warning(disable:4786) + #define SIMD_FORCE_INLINE __forceinline + #endif //__MINGW32__ + + //#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED16(a) a + #include <assert.h> + #define ASSERT assert +#else + + //non-windows systems + + #define SIMD_FORCE_INLINE inline + #define ATTRIBUTE_ALIGNED16(a) a + #ifndef assert + #include <assert.h> + #endif + #define ASSERT assert +#endif + + + +typedef float btScalar; + +#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__) +//use double float precision operation on those platforms for Blender + +SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); } +SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); } +SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cos(x); } +SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sin(x); } +SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tan(x); } +SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acos(x); } +SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asin(x); } +SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atan(x); } +SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2(x, y); } +SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return exp(x); } +SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return log(x); } +SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); } + +#else + +SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrtf(x); } +SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabsf(x); } +SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } +SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } +SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } +SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); } +SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } +SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } +SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } +SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); } +SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); } +SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } + +#endif + + +const btScalar SIMD_2_PI = 6.283185307179586232f; +const btScalar SIMD_PI = SIMD_2_PI * btScalar(0.5f); +const btScalar SIMD_HALF_PI = SIMD_2_PI * btScalar(0.25f); +const btScalar SIMD_RADS_PER_DEG = SIMD_2_PI / btScalar(360.0f); +const btScalar SIMD_DEGS_PER_RAD = btScalar(360.0f) / SIMD_2_PI; +const btScalar SIMD_EPSILON = FLT_EPSILON; +const btScalar SIMD_INFINITY = FLT_MAX; + +SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } + +SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { + return (((a) <= eps) && !((a) < -eps)); +} +SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { + return (!((a) <= eps)); +} + +/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } +SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } +SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } +SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); } +SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } +SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } +SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } +*/ + +SIMD_FORCE_INLINE int btSign(btScalar x) { + return x < 0.0f ? -1 : x > 0.0f ? 1 : 0; +} + +SIMD_FORCE_INLINE btScalar btRadians(btScalar x) { return x * SIMD_RADS_PER_DEG; } +SIMD_FORCE_INLINE btScalar btDegrees(btScalar x) { return x * SIMD_DEGS_PER_RAD; } + + + +#endif //SIMD___SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btSimdMinMax.h b/extern/bullet2/src/LinearMath/btSimdMinMax.h new file mode 100644 index 00000000000..16c31552f88 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btSimdMinMax.h @@ -0,0 +1,40 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef SIMD_MINMAX_H +#define SIMD_MINMAX_H + +template <class T> +SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { + return b < a ? b : a; +} + +template <class T> +SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) { + return a < b ? b : a; +} + +template <class T> +SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) { + if (a > b) a = b; +} + +template <class T> +SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) { + if (a < b) a = b; +} + +#endif diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h new file mode 100644 index 00000000000..3f9a48407c7 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -0,0 +1,193 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef btTransform_H +#define btTransform_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + + +///btTransform supports rigid transforms (only translation and rotation, no scaling/shear) +class btTransform { + + +public: + + + btTransform() {} + + explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q, + const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) + : m_basis(q), + m_origin(c) + {} + + explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b, + const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0))) + : m_basis(b), + m_origin(c) + {} + + + SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { + m_basis = t1.m_basis * t2.m_basis; + m_origin = t1(t2.m_origin); + } + + void multInverseLeft(const btTransform& t1, const btTransform& t2) { + btVector3 v = t2.m_origin - t1.m_origin; + m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis); + m_origin = v * t1.m_basis; + } + + SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const + { + return btVector3(m_basis[0].dot(x) + m_origin[0], + m_basis[1].dot(x) + m_origin[1], + m_basis[2].dot(x) + m_origin[2]); + } + + SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const + { + return (*this)(x); + } + + SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; } + SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; } + + SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; } + SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; } + + btQuaternion getRotation() const { + btQuaternion q; + m_basis.getRotation(q); + return q; + } + template <typename Scalar2> + void setValue(const Scalar2 *m) + { + m_basis.setValue(m); + m_origin.setValue(&m[12]); + } + + + void setFromOpenGLMatrix(const btScalar *m) + { + m_basis.setFromOpenGLSubMatrix(m); + m_origin[0] = m[12]; + m_origin[1] = m[13]; + m_origin[2] = m[14]; + } + + void getOpenGLMatrix(btScalar *m) const + { + m_basis.getOpenGLSubMatrix(m); + m[12] = m_origin[0]; + m[13] = m_origin[1]; + m[14] = m_origin[2]; + m[15] = btScalar(1.0f); + } + + SIMD_FORCE_INLINE void setOrigin(const btVector3& origin) + { + m_origin = origin; + } + + SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const; + + + + SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis) + { + m_basis = basis; + } + + SIMD_FORCE_INLINE void setRotation(const btQuaternion& q) + { + m_basis.setRotation(q); + } + + + + void setIdentity() + { + m_basis.setIdentity(); + m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); + } + + + btTransform& operator*=(const btTransform& t) + { + m_origin += m_basis * t.m_origin; + m_basis *= t.m_basis; + return *this; + } + + btTransform inverse() const + { + btMatrix3x3 inv = m_basis.transpose(); + return btTransform(inv, inv * -m_origin); + } + + btTransform inverseTimes(const btTransform& t) const; + + btTransform operator*(const btTransform& t) const; + + static btTransform getIdentity() + { + btTransform tr; + tr.setIdentity(); + return tr; + } + +private: + + btMatrix3x3 m_basis; + btVector3 m_origin; +}; + + +SIMD_FORCE_INLINE btVector3 +btTransform::invXform(const btVector3& inVec) const +{ + btVector3 v = inVec - m_origin; + return (m_basis.transpose() * v); +} + +SIMD_FORCE_INLINE btTransform +btTransform::inverseTimes(const btTransform& t) const +{ + btVector3 v = t.getOrigin() - m_origin; + return btTransform(m_basis.transposeTimes(t.m_basis), + v * m_basis); +} + +SIMD_FORCE_INLINE btTransform +btTransform::operator*(const btTransform& t) const +{ + return btTransform(m_basis * t.m_basis, + (*this)(t.m_origin)); +} + + + +#endif + + + + + diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h new file mode 100644 index 00000000000..da8e4aa72a8 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -0,0 +1,143 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef SIMD_TRANSFORM_UTIL_H +#define SIMD_TRANSFORM_UTIL_H + +#include "LinearMath/btTransform.h" +#define ANGULAR_MOTION_TRESHOLD 0.5f*SIMD_HALF_PI + + + +#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490) + +#define btRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ + +inline btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir) +{ + return btVector3(supportDir.x() < btScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + supportDir.y() < btScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + supportDir.z() < btScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); +} + + +inline void btPlaneSpace1 (const btVector3& n, btVector3& p, btVector3& q) +{ + if (btFabs(n[2]) > SIMDSQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = btRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar k = btRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + + +/// Utils related to temporal transforms +class btTransformUtil +{ + +public: + + static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform) + { + predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep); +// #define QUATERNION_DERIVATIVE + #ifdef QUATERNION_DERIVATIVE + btQuaternion orn = curTrans.getRotation(); + orn += (angvel * orn) * (timeStep * 0.5f); + orn.normalize(); + #else + //exponential map + btVector3 axis; + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle*timeStep > ANGULAR_MOTION_TRESHOLD) + { + fAngle = ANGULAR_MOTION_TRESHOLD / timeStep; + } + + if ( fAngle < 0.001f ) + { + // use Taylor's expansions of sync function + axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle ); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel*( btSin(0.5f*fAngle*timeStep)/fAngle ); + } + btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*0.5f )); + btQuaternion orn0 = curTrans.getRotation(); + + btQuaternion predictedOrn = dorn * orn0; + #endif + predictedTransform.setRotation(predictedOrn); + } + + static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel) + { + linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep; +#ifdef USE_QUATERNION_DIFF + btQuaternion orn0 = transform0.getRotation(); + btQuaternion orn1a = transform1.getRotation(); + btQuaternion orn1 = orn0.farthest(orn1a); + btQuaternion dorn = orn1 * orn0.inverse(); +#else + btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse(); + btQuaternion dorn; + dmat.getRotation(dorn); +#endif//USE_QUATERNION_DIFF + + btVector3 axis; + btScalar angle; + angle = dorn.getAngle(); + axis = btVector3(dorn.x(),dorn.y(),dorn.z()); + axis[3] = 0.f; + //check for axis length + btScalar len = axis.length2(); + if (len < SIMD_EPSILON*SIMD_EPSILON) + axis = btVector3(1.f,0.f,0.f); + else + axis /= btSqrt(len); + + + angVel = axis * angle / timeStep; + + } + + +}; + +#endif //SIMD_TRANSFORM_UTIL_H + diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h new file mode 100644 index 00000000000..5a35652ecd3 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -0,0 +1,403 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef SIMD__VECTOR3_H +#define SIMD__VECTOR3_H + +#include "btQuadWord.h" + + +///btVector3 is 16byte aligned, and has an extra unused component m_w +///this extra component can be used by derived classes (Quaternion?) or by user +class btVector3 : public btQuadWord { + + +public: + SIMD_FORCE_INLINE btVector3() {} + + + + SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) + :btQuadWord(x,y,z,0.f) + { + } + +// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) +// : btQuadWord(x,y,z,w) +// { +// } + + + + SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) + { + m_x += v.x(); m_y += v.y(); m_z += v.z(); + return *this; + } + + + + SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) + { + m_x -= v.x(); m_y -= v.y(); m_z -= v.z(); + return *this; + } + + SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) + { + m_x *= s; m_y *= s; m_z *= s; + return *this; + } + + SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) + { + assert(s != btScalar(0.0)); + return *this *= btScalar(1.0) / s; + } + + SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const + { + return m_x * v.x() + m_y * v.y() + m_z * v.z(); + } + + SIMD_FORCE_INLINE btScalar length2() const + { + return dot(*this); + } + + SIMD_FORCE_INLINE btScalar length() const + { + return btSqrt(length2()); + } + + SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; + + SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const; + + SIMD_FORCE_INLINE btVector3& normalize() + { + return *this /= length(); + } + + SIMD_FORCE_INLINE btVector3 normalized() const; + + SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ); + + SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const + { + btScalar s = btSqrt(length2() * v.length2()); + assert(s != btScalar(0.0)); + return btAcos(dot(v) / s); + } + + SIMD_FORCE_INLINE btVector3 absolute() const + { + return btVector3( + btFabs(m_x), + btFabs(m_y), + btFabs(m_z)); + } + + SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const + { + return btVector3( + m_y * v.z() - m_z * v.y(), + m_z * v.x() - m_x * v.z(), + m_x * v.y() - m_y * v.x()); + } + + SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const + { + return m_x * (v1.y() * v2.z() - v1.z() * v2.y()) + + m_y * (v1.z() * v2.x() - v1.x() * v2.z()) + + m_z * (v1.x() * v2.y() - v1.y() * v2.x()); + } + + SIMD_FORCE_INLINE int minAxis() const + { + return m_x < m_y ? (m_x < m_z ? 0 : 2) : (m_y < m_z ? 1 : 2); + } + + SIMD_FORCE_INLINE int maxAxis() const + { + return m_x < m_y ? (m_y < m_z ? 2 : 1) : (m_x < m_z ? 2 : 0); + } + + SIMD_FORCE_INLINE int furthestAxis() const + { + return absolute().minAxis(); + } + + SIMD_FORCE_INLINE int closestAxis() const + { + return absolute().maxAxis(); + } + + SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) + { + btScalar s = 1.0f - rt; + m_x = s * v0[0] + rt * v1.x(); + m_y = s * v0[1] + rt * v1.y(); + m_z = s * v0[2] + rt * v1.z(); + //don't do the unused w component + // m_co[3] = s * v0[3] + rt * v1[3]; + } + + SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const + { + return btVector3(m_x + (v.x() - m_x) * t, + m_y + (v.y() - m_y) * t, + m_z + (v.z() - m_z) * t); + } + + + SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) + { + m_x *= v.x(); m_y *= v.y(); m_z *= v.z(); + return *this; + } + + + +}; + +SIMD_FORCE_INLINE btVector3 +operator+(const btVector3& v1, const btVector3& v2) +{ + return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); +} + +SIMD_FORCE_INLINE btVector3 +operator*(const btVector3& v1, const btVector3& v2) +{ + return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); +} + +SIMD_FORCE_INLINE btVector3 +operator-(const btVector3& v1, const btVector3& v2) +{ + return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); +} + +SIMD_FORCE_INLINE btVector3 +operator-(const btVector3& v) +{ + return btVector3(-v.x(), -v.y(), -v.z()); +} + +SIMD_FORCE_INLINE btVector3 +operator*(const btVector3& v, const btScalar& s) +{ + return btVector3(v.x() * s, v.y() * s, v.z() * s); +} + +SIMD_FORCE_INLINE btVector3 +operator*(const btScalar& s, const btVector3& v) +{ + return v * s; +} + +SIMD_FORCE_INLINE btVector3 +operator/(const btVector3& v, const btScalar& s) +{ + assert(s != btScalar(0.0)); + return v * (btScalar(1.0) / s); +} + +SIMD_FORCE_INLINE btVector3 +operator/(const btVector3& v1, const btVector3& v2) +{ + return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); +} + +SIMD_FORCE_INLINE btScalar +dot(const btVector3& v1, const btVector3& v2) +{ + return v1.dot(v2); +} + + + +SIMD_FORCE_INLINE btScalar +distance2(const btVector3& v1, const btVector3& v2) +{ + return v1.distance2(v2); +} + + +SIMD_FORCE_INLINE btScalar +distance(const btVector3& v1, const btVector3& v2) +{ + return v1.distance(v2); +} + +SIMD_FORCE_INLINE btScalar +angle(const btVector3& v1, const btVector3& v2) +{ + return v1.angle(v2); +} + +SIMD_FORCE_INLINE btVector3 +cross(const btVector3& v1, const btVector3& v2) +{ + return v1.cross(v2); +} + +SIMD_FORCE_INLINE btScalar +triple(const btVector3& v1, const btVector3& v2, const btVector3& v3) +{ + return v1.triple(v2, v3); +} + +SIMD_FORCE_INLINE btVector3 +lerp(const btVector3& v1, const btVector3& v2, const btScalar& t) +{ + return v1.lerp(v2, t); +} + + +SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2) +{ + return p1[0] == p2[0] && p1[1] == p2[1] && p1[2] == p2[2]; +} + +SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const +{ + return (v - *this).length2(); +} + +SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const +{ + return (v - *this).length(); +} + +SIMD_FORCE_INLINE btVector3 btVector3::normalized() const +{ + return *this / length(); +} + +SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar angle ) +{ + // wAxis must be a unit lenght vector + + btVector3 o = wAxis * wAxis.dot( *this ); + btVector3 x = *this - o; + btVector3 y; + + y = wAxis.cross( *this ); + + return ( o + x * btCos( angle ) + y * btSin( angle ) ); +} + +class btVector4 : public btVector3 +{ +public: + + SIMD_FORCE_INLINE btVector4() {} + + + SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) + : btVector3(x,y,z) + { + m_unusedW = w; + } + + + SIMD_FORCE_INLINE btVector4 absolute4() const + { + return btVector4( + btFabs(m_x), + btFabs(m_y), + btFabs(m_z), + btFabs(m_unusedW)); + } + + + + float getW() const { return m_unusedW;} + + + SIMD_FORCE_INLINE int maxAxis4() const + { + int maxIndex = -1; + float maxVal = -1e30f; + if (m_x > maxVal) + { + maxIndex = 0; + maxVal = m_x; + } + if (m_y > maxVal) + { + maxIndex = 1; + maxVal = m_y; + } + if (m_z > maxVal) + { + maxIndex = 2; + maxVal = m_z; + } + if (m_unusedW > maxVal) + { + maxIndex = 3; + maxVal = m_unusedW; + } + + + + + return maxIndex; + + } + + + SIMD_FORCE_INLINE int minAxis4() const + { + int minIndex = -1; + float minVal = 1e30f; + if (m_x < minVal) + { + minIndex = 0; + minVal = m_x; + } + if (m_y < minVal) + { + minIndex = 1; + minVal = m_y; + } + if (m_z < minVal) + { + minIndex = 2; + minVal = m_z; + } + if (m_unusedW < minVal) + { + minIndex = 3; + minVal = m_unusedW; + } + + return minIndex; + + } + + + SIMD_FORCE_INLINE int closestAxis4() const + { + return absolute4().maxAxis4(); + } + +}; + +#endif //SIMD__VECTOR3_H diff --git a/extern/bullet2/src/btBulletCollisionCommon.h b/extern/bullet2/src/btBulletCollisionCommon.h new file mode 100644 index 00000000000..6ee7941dfcf --- /dev/null +++ b/extern/bullet2/src/btBulletCollisionCommon.h @@ -0,0 +1,59 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BULLET_COLLISION_COMMON_H +#define BULLET_COLLISION_COMMON_H + +///Common headerfile includes for Bullet Collision Detection + +///Bullet's btCollisionWorld and btCollisionObject definitions +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" + +///Collision Shapes +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" +#include "BulletCollision/CollisionShapes/btConvexHullShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMesh.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btTetrahedronShape.h" +#include "BulletCollision/CollisionShapes/btEmptyShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" + +///Narrowphase Collision Detector +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" + +///Dispatching and generation of collision pairs (broadphase) +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btAxisSweep3.h" + + +///Math library +#include "LinearMath/btQuaternion.h" +#include "LinearMath/btTransform.h" + + + +#endif //BULLET_COLLISION_COMMON_H + diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h new file mode 100644 index 00000000000..051ee8ce0bf --- /dev/null +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -0,0 +1,38 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BULLET_DYNAMICS_COMMON_H +#define BULLET_DYNAMICS_COMMON_H + +///Common headerfile includes for Bullet Dynamics, including Collision Detection +#include "btBulletCollisionCommon.h" + +#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" +#include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" + +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" + + +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +///Vehicle simulation, with wheel contact simulated by raycasts +#include "BulletDynamics/Vehicle/btRaycastVehicle.h" + + + +#endif //BULLET_DYNAMICS_COMMON_H + |