commit e061ec1ebfbf3fd331e8a01ba2b05be123884639 Author: ejcoumans Date: Thu May 25 19:18:29 2006 +0000 moved files around diff --git a/.cvsignore b/.cvsignore new file mode 100644 index 000000000..767a9cc5d --- /dev/null +++ b/.cvsignore @@ -0,0 +1,22 @@ +*.exe +*.ilk +autom4te.cache +bullet.pc +config.cache +config.h +config.h.in~ +config.log +config.status +out +Jamconfig +Jamfile +CcdPhysicsDemo +CollisionDemo +CollisionInterfaceDemo +ConcaveDemo +ConstraintDemo +ContinuousConvexCollision +ConvexHullDistance +GjkConvexCastDemo +Raytracer +SimplexDemo diff --git a/Bullet/BroadphaseCollision/AxisSweep3.cpp b/Bullet/BroadphaseCollision/AxisSweep3.cpp new file mode 100644 index 000000000..25abf92ae --- /dev/null +++ b/Bullet/BroadphaseCollision/AxisSweep3.cpp @@ -0,0 +1,499 @@ + +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + + +// +// AxisSweep3 +// +// 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 "AxisSweep3.h" + +#include + +BroadphaseProxy* AxisSweep3::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ) +{ + unsigned short handleId = AddHandle(min,max, userPtr); + + Handle* handle = GetHandle(handleId); + return handle; +} + +void AxisSweep3::DestroyProxy(BroadphaseProxy* proxy) +{ + Handle* handle = static_cast(proxy); + RemoveHandle(handle->m_handleId); +} + +void AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax) +{ + Handle* handle = static_cast(proxy); + UpdateHandle(handle->m_handleId,aabbMin,aabbMax); +} + + + + + + +AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps) +{ + //assert(bounds.HasVolume()); + + // 1 handle is reserved as sentinel + assert(maxHandles > 1 && maxHandles < 32767); + + // doesn't need this limit right now, but I may want to use unsigned short indexes into overlaps array + assert(maxOverlaps > 0 && maxOverlaps < 65536); + + // init bounds + m_worldAabbMin = worldAabbMin; + m_worldAabbMax = worldAabbMax; + + SimdVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; + + m_quantize = SimdVector3(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; + } +} + +AxisSweep3::~AxisSweep3() +{ + + for (int i = 2; i >= 0; i--) + delete[] m_pEdges[i]; + delete[] m_pHandles; +} + +void AxisSweep3::Quantize(unsigned short* out, const SimdPoint3& point, int isMax) const +{ + SimdPoint3 clampedPoint(point); + /* + if (isMax) + clampedPoint += SimdVector3(10,10,10); + else + { + clampedPoint -= SimdVector3(10,10,10); + } + */ + + + clampedPoint.setMax(m_worldAabbMin); + clampedPoint.setMin(m_worldAabbMax); + + SimdVector3 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 AxisSweep3::AllocHandle() +{ + assert(m_firstFreeHandle); + + unsigned short handle = m_firstFreeHandle; + m_firstFreeHandle = GetHandle(handle)->GetNextFree(); + m_numHandles++; + + return handle; +} + +void AxisSweep3::FreeHandle(unsigned short handle) +{ + assert(handle > 0 && handle < m_maxHandles); + + GetHandle(handle)->SetNextFree(m_firstFreeHandle); + m_firstFreeHandle = handle; + + m_numHandles--; +} + + + +unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner) +{ + // 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; + + // 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 AxisSweep3::RemoveHandle(unsigned short handle) +{ + Handle* pHandle = GetHandle(handle); + + 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 AxisSweep3::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 AxisSweep3::UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& 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 AxisSweep3::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 AxisSweep3::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); + BroadphasePair* pair = FindPair(handle0,handle1); + //assert(pair); + if (pair) + { + RemoveOverlappingPair(*pair); + } + } + + // 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 AxisSweep3::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); + BroadphasePair* 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 AxisSweep3::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/Bullet/BroadphaseCollision/AxisSweep3.h b/Bullet/BroadphaseCollision/AxisSweep3.h new file mode 100644 index 000000000..0a2cb55de --- /dev/null +++ b/Bullet/BroadphaseCollision/AxisSweep3.h @@ -0,0 +1,115 @@ +//Bullet Continuous Collision Detection and Physics Library +//Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +// +// AxisSweep3.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 "SimdPoint3.h" +#include "SimdVector3.h" +#include "SimpleBroadphase.h" +#include "BroadphaseProxy.h" + +/// AxisSweep3 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 AxisSweep3 : public SimpleBroadphase +{ + +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 BroadphaseProxy + { + 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 BroadphaseProxy.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: + SimdPoint3 m_worldAabbMin; // overall system bounds + SimdPoint3 m_worldAabbMax; // overall system bounds + + SimdVector3 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 SimdPoint3& 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: + AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles = 1024, int maxOverlaps = 8192); + virtual ~AxisSweep3(); + + virtual void RefreshOverlappingPairs() + { + //this is replace by sweep and prune + } + + unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner); + void RemoveHandle(unsigned short handle); + void UpdateHandle(unsigned short handle, const SimdPoint3& aabbMin,const SimdPoint3& aabbMax); + inline Handle* GetHandle(unsigned short index) const {return m_pHandles + index;} + + + //Broadphase Interface + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ); + virtual void DestroyProxy(BroadphaseProxy* proxy); + virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax); + +}; + +#endif //AXIS_SWEEP_3_H diff --git a/Bullet/BroadphaseCollision/BroadphaseInterface.h b/Bullet/BroadphaseCollision/BroadphaseInterface.h new file mode 100644 index 000000000..5b9efc6b9 --- /dev/null +++ b/Bullet/BroadphaseCollision/BroadphaseInterface.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 DispatcherInfo; +class Dispatcher; +struct BroadphaseProxy; +#include "SimdVector3.h" + +///BroadphaseInterface for aabb-overlapping object pairs +class BroadphaseInterface +{ +public: + virtual ~BroadphaseInterface() {} + + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ) =0; + virtual void DestroyProxy(BroadphaseProxy* proxy)=0; + virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)=0; + virtual void CleanProxyFromPairs(BroadphaseProxy* proxy)=0; + virtual void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)=0; + +}; + +#endif //BROADPHASE_INTERFACE_H diff --git a/Bullet/BroadphaseCollision/BroadphaseProxy.cpp b/Bullet/BroadphaseCollision/BroadphaseProxy.cpp new file mode 100644 index 000000000..61bcdc4b6 --- /dev/null +++ b/Bullet/BroadphaseCollision/BroadphaseProxy.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 "BroadphaseProxy.h" + diff --git a/Bullet/BroadphaseCollision/BroadphaseProxy.h b/Bullet/BroadphaseCollision/BroadphaseProxy.h new file mode 100644 index 000000000..78bd7ab76 --- /dev/null +++ b/Bullet/BroadphaseCollision/BroadphaseProxy.h @@ -0,0 +1,116 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 + + + +/// Dispatcher 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_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, + EMPTY_SHAPE_PROXYTYPE, + + MAX_BROADPHASE_COLLISION_TYPES +}; + + +///BroadphaseProxy +struct BroadphaseProxy +{ + + //Usually the client CollisionObject or Rigidbody class + void* m_clientObject; + + + BroadphaseProxy() :m_clientObject(0){} + BroadphaseProxy(int shapeType,void* userPtr) + :m_clientObject(userPtr) + //m_clientObjectType(shapeType) + { + } + +}; + +class CollisionAlgorithm; + +struct BroadphaseProxy; + +#define SIMPLE_MAX_ALGORITHMS 2 + +/// contains a pair of aabb-overlapping objects +struct BroadphasePair +{ + BroadphasePair () + : + m_pProxy0(0), + m_pProxy1(0) + { + for (int i=0;i + + +void SimpleBroadphase::validate() +{ + for (int i=0;i=0;i--) + { + BP_Proxy* proxy = m_pProxies[i]; + destroyProxy(proxy); + } + */ +} + + +BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr) +{ + 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]; + SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr); + m_firstFreeProxy++; + + SimpleBroadphaseProxy* proxy1 = &m_proxies[0]; + + int index = proxy - proxy1; + assert(index == freeIndex); + + m_pProxies[m_numProxies] = proxy; + m_numProxies++; + //validate(); + + return proxy; +} + +void SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy) +{ + int i; + for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--) + { + BroadphasePair& pair = m_OverlappingPairs[i]; + if (pair.m_pProxy0 == proxy || + pair.m_pProxy1 == proxy) + { + RemoveOverlappingPair(pair); + } + } +} + +void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg) +{ + + int i; + + SimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); + SimpleBroadphaseProxy* proxy1 = &m_proxies[0]; + + int index = proxy0 - proxy1; + assert (index < m_maxProxies); + m_freeProxies[--m_firstFreeProxy] = index; + + RemoveOverlappingPairsContainingProxy(proxyOrg); + + + for (i=0;im_min = aabbMin; + sbp->m_max = aabbMax; +} + +void SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair) +{ + for (int dispatcherId=0;dispatcherId= m_maxOverlap) + { + //printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair); +#ifdef DEBUG + assert(0); +#endif + } else + { + m_NumOverlapBroadphasePair++; + } + + +} + +BroadphasePair* SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +{ + BroadphasePair* foundPair = 0; + + int i; + for (i=m_NumOverlapBroadphasePair-1;i>=0;i--) + { + BroadphasePair& pair = m_OverlappingPairs[i]; + if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) || + ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0))) + { + foundPair = &pair; + return foundPair; + } + } + + return foundPair; +} +void SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair) +{ + CleanOverlappingPair(pair); + int index = &pair - &m_OverlappingPairs[0]; + //remove efficiently, swap with the last + m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1]; + m_NumOverlapBroadphasePair--; +} + +bool SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* 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]; + +} +void SimpleBroadphase::RefreshOverlappingPairs() +{ + //first check for new overlapping pairs + int i,j; + + for (i=0;i= 0) + { + //dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithms[dispatcherId]) + { + pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + } + + if (pair.m_algorithms[dispatcherId]) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } else + { + //non-persistent algorithm dispatcher + CollisionAlgorithm* algo = dispatcher.FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + + if (algo) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + } + } + } + + } + + m_blockedForChanges = false; + +} + + diff --git a/Bullet/BroadphaseCollision/SimpleBroadphase.h b/Bullet/BroadphaseCollision/SimpleBroadphase.h new file mode 100644 index 000000000..b61f82b62 --- /dev/null +++ b/Bullet/BroadphaseCollision/SimpleBroadphase.h @@ -0,0 +1,97 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 + +//#define SIMPLE_MAX_PROXIES 8192 +//#define SIMPLE_MAX_OVERLAP 4096 + +#include "BroadphaseInterface.h" +#include "BroadphaseProxy.h" +#include "SimdPoint3.h" + +struct SimpleBroadphaseProxy : public BroadphaseProxy +{ + SimdVector3 m_min; + SimdVector3 m_max; + + SimpleBroadphaseProxy() {}; + + SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr) + :BroadphaseProxy(shapeType,userPtr), + m_min(minpt),m_max(maxpt) + { + } + + +}; + +///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks +class SimpleBroadphase : public BroadphaseInterface +{ + + SimpleBroadphaseProxy* m_proxies; + int* m_freeProxies; + int m_firstFreeProxy; + + SimpleBroadphaseProxy** m_pProxies; + int m_numProxies; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + BroadphasePair* m_OverlappingPairs; + int m_NumOverlapBroadphasePair; + + int m_maxProxies; + int m_maxOverlap; + + inline SimpleBroadphaseProxy* GetSimpleProxyFromProxy(BroadphaseProxy* proxy) + { + SimpleBroadphaseProxy* proxy0 = static_cast(proxy); + return proxy0; + } + + bool AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1); + + void validate(); + +protected: + void RemoveOverlappingPair(BroadphasePair& pair); + void CleanOverlappingPair(BroadphasePair& pair); + + void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy); + + void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + virtual void RefreshOverlappingPairs(); +public: + SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192); + virtual ~SimpleBroadphase(); + + virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr); + + virtual void DestroyProxy(BroadphaseProxy* proxy); + virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax); + virtual void CleanProxyFromPairs(BroadphaseProxy* proxy); + virtual void DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo); + +}; + + + +#endif //SIMPLE_BROADPHASE_H + diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/Bullet/CollisionDispatch/CollisionDispatcher.cpp new file mode 100644 index 000000000..0c7467817 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -0,0 +1,300 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "CollisionDispatcher.h" + + +#include "BroadphaseCollision/CollisionAlgorithm.h" +#include "CollisionDispatch/ConvexConvexAlgorithm.h" +#include "CollisionDispatch/EmptyCollisionAlgorithm.h" +#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h" +#include "CollisionShapes/CollisionShape.h" +#include "CollisionDispatch/CollisionObject.h" +#include + +int gNumManifold = 0; + +void CollisionDispatcher::FindUnions() +{ + if (m_useIslands) + { + for (int i=0;iGetManifoldByIndexInternal(i); + //static objects (invmass 0.f) don't merge ! + + const CollisionObject* colObj0 = static_cast(manifold->GetBody0()); + const CollisionObject* colObj1 = static_cast(manifold->GetBody1()); + + if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1)) + { + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + + m_unionFind.unite((colObj0)->m_islandTag1, + (colObj1)->m_islandTag1); + } + } + + + } + } + +} + + + +CollisionDispatcher::CollisionDispatcher (): + m_useIslands(true), + m_defaultManifoldResult(0,0,0), + m_count(0) +{ + int i; + + for (i=0;iClearManifold(); +} + + +void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold) +{ + + gNumManifold--; + + //printf("ReleaseManifold: gNumManifold %d\n",gNumManifold); + + ClearManifold(manifold); + + std::vector::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; + + } + + +} + + +// +// todo: this is random access, it can be walked 'cache friendly'! +// +void CollisionDispatcher::BuildAndProcessIslands(int numBodies, IslandCallback* callback) +{ + for (int islandId=0;islandId islandmanifold; + + //int numSleeping = 0; + + bool allSleeping = true; + + for (int i=0;iGetManifoldByIndexInternal(i); + + //filtering for response + + CollisionObject* colObj0 = static_cast(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast(manifold->GetBody1()); + + + + { + if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) || + ((colObj1) && (colObj1)->m_islandTag1 == (islandId))) + { + + if (((colObj0) && (colObj0)->GetActivationState()== ACTIVE_TAG) || + ((colObj1) && (colObj1)->GetActivationState() == ACTIVE_TAG)) + { + allSleeping = false; + } + if (((colObj0) && (colObj0)->GetActivationState()== DISABLE_DEACTIVATION) || + ((colObj1) && (colObj1)->GetActivationState() == DISABLE_DEACTIVATION)) + { + allSleeping = false; + } + + if (NeedsResponse(*colObj0,*colObj1)) + islandmanifold.push_back(manifold); + } + } + } + if (allSleeping) + { + //tag all as 'ISLAND_SLEEPING' + for (size_t i=0;i(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast(manifold->GetBody1()); + + if ((colObj0)) + { + (colObj0)->SetActivationState( ISLAND_SLEEPING ); + } + if ((colObj1)) + { + (colObj1)->SetActivationState( ISLAND_SLEEPING); + } + + } + } else + { + + //tag all as 'ISLAND_SLEEPING' + for (size_t i=0;iGetBody0(); + CollisionObject* body1 = (CollisionObject*)manifold->GetBody1(); + + if (body0) + { + if ( body0->GetActivationState() == ISLAND_SLEEPING) + { + body0->SetActivationState( WANTS_DEACTIVATION); + } + } + if (body1) + { + if ( body1->GetActivationState() == ISLAND_SLEEPING) + { + body1->SetActivationState(WANTS_DEACTIVATION); + } + } + + } + + if (islandmanifold.size()) + { + callback->ProcessIsland(&islandmanifold[0],islandmanifold.size()); + } + + } + } +} + + + +CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) +{ + m_count++; + CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; + CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; + + CollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = this; + + if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() ) + { + return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1); + } + + if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1); + } + + if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave()) + { + return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0); + } + + //failed to find an algorithm + return new EmptyAlgorithm(ci); + +} + +bool CollisionDispatcher::NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1) +{ + //here you can do filtering + bool hasResponse = + (!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) && + (!(colObj1.m_collisionFlags & CollisionObject::noContactResponse)); + return hasResponse; +} + +bool CollisionDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) +{ + + CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject; + CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject; + + assert(body0); + assert(body1); + + bool needsCollision = true; + + if ((body0->m_collisionFlags & CollisionObject::isStatic) && + (body1->m_collisionFlags & CollisionObject::isStatic)) + needsCollision = false; + + if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2)) + needsCollision = false; + + return needsCollision ; + +} + +///allows the user to get contact point callbacks +ManifoldResult* CollisionDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) +{ + + + //in-place, this prevents parallel dispatching, but just adding a list would fix that. + ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold); + return manifoldResult; +} + +///allows the user to get contact point callbacks +void CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*) +{ + +} diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.h b/Bullet/CollisionDispatch/CollisionDispatcher.h new file mode 100644 index 000000000..3896c19e1 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionDispatcher.h @@ -0,0 +1,139 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BroadphaseCollision/Dispatcher.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "CollisionDispatch/UnionFind.h" +#include "CollisionDispatch/ManifoldResult.h" + +#include "BroadphaseCollision/BroadphaseProxy.h" +#include + +class IDebugDraw; + + + + +struct CollisionAlgorithmCreateFunc +{ + bool m_swapped; + + CollisionAlgorithmCreateFunc() + :m_swapped(false) + { + } + virtual ~CollisionAlgorithmCreateFunc(){}; + + virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) + { + return 0; + } +}; + + +///CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. +///Time of Impact, Closest Points and Penetration Depth. +class CollisionDispatcher : public Dispatcher +{ + + std::vector m_manifoldsPtr; + + UnionFind m_unionFind; + + bool m_useIslands; + + ManifoldResult m_defaultManifoldResult; + + CollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + +public: + + UnionFind& GetUnionFind() { return m_unionFind;} + + struct IslandCallback + { + virtual ~IslandCallback() {}; + + virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0; + }; + + + int GetNumManifolds() const + { + return m_manifoldsPtr.size(); + } + + PersistentManifold* GetManifoldByIndexInternal(int index) + { + return m_manifoldsPtr[index]; + } + + const PersistentManifold* GetManifoldByIndexInternal(int index) const + { + return m_manifoldsPtr[index]; + } + + void InitUnionFind(int n) + { + if (m_useIslands) + m_unionFind.reset(n); + } + + void FindUnions(); + + int m_count; + + CollisionDispatcher (); + virtual ~CollisionDispatcher() {}; + + virtual PersistentManifold* GetNewManifold(void* b0,void* b1); + + virtual void ReleaseManifold(PersistentManifold* manifold); + + + virtual void BuildAndProcessIslands(int numBodies, IslandCallback* callback); + + ///allows the user to get contact point callbacks + virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold); + + ///allows the user to get contact point callbacks + virtual void ReleaseManifoldResult(ManifoldResult*); + + virtual void ClearManifold(PersistentManifold* manifold); + + + CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) + { + CollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1); + return algo; + } + + CollisionAlgorithm* InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); + + virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1); + + virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1); + + virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} + + + +}; + +#endif //COLLISION__DISPATCHER_H + diff --git a/Bullet/CollisionDispatch/CollisionObject.cpp b/Bullet/CollisionDispatch/CollisionObject.cpp new file mode 100644 index 000000000..4b123ebe0 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionObject.cpp @@ -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. +*/ + +#include "CollisionObject.h" + +CollisionObject::CollisionObject() + : m_collisionFlags(0), + m_activationState1(1), + m_deactivationTime(0.f), + m_broadphaseHandle(0), + m_collisionShape(0), + m_hitFraction(1.f), + m_ccdSweptShereRadius(0.f), + m_ccdSquareMotionTreshold(0.f) +{ + +} + + +void CollisionObject::SetActivationState(int newState) +{ + if (m_activationState1 != DISABLE_DEACTIVATION) + m_activationState1 = newState; +} + +void CollisionObject::ForceActivationState(int newState) +{ + m_activationState1 = newState; +} + +void CollisionObject::activate() +{ + if (!(m_collisionFlags & isStatic)) + { + SetActivationState(1); + m_deactivationTime = 0.f; + } +} + +bool CollisionObject::mergesSimulationIslands() const +{ + return ( !(m_collisionFlags & isStatic)); +} diff --git a/Bullet/CollisionDispatch/CollisionObject.h b/Bullet/CollisionDispatch/CollisionObject.h new file mode 100644 index 000000000..e8940a9b0 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionObject.h @@ -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. +*/ + +#ifndef COLLISION_OBJECT_H +#define COLLISION_OBJECT_H + +#include "SimdTransform.h" + +//island management, m_activationState1 +#define ACTIVE_TAG 1 +#define ISLAND_SLEEPING 2 +#define WANTS_DEACTIVATION 3 +#define DISABLE_DEACTIVATION 4 + +struct BroadphaseProxy; +class CollisionShape; + +/// CollisionObject can be used to manage collision detection objects. +/// CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. +/// They can be added to the CollisionWorld. +struct CollisionObject +{ + SimdTransform m_worldTransform; + + //m_interpolationWorldTransform is used for CCD and interpolation + //it can be either previous or future (predicted) transform + SimdTransform m_interpolationWorldTransform; + + enum CollisionFlags + { + isStatic = 1, + noContactResponse = 2, + + }; + + int m_collisionFlags; + + int m_islandTag1; + int m_activationState1; + float m_deactivationTime; + + BroadphaseProxy* m_broadphaseHandle; + CollisionShape* m_collisionShape; + + void* m_userPointer;//not use by Bullet internally + + ///time of impact calculation + float m_hitFraction; + + ///Swept sphere radius (0.0 by default), see ConvexConvexAlgorithm:: + float m_ccdSweptShereRadius; + + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold + float m_ccdSquareMotionTreshold; + + bool mergesSimulationIslands() const; + + inline bool IsStatic() const { + return m_collisionFlags & isStatic; + } + + inline bool HasContactResponse() { + return !(m_collisionFlags & noContactResponse); + } + + + + + CollisionObject(); + + + void SetCollisionShape(CollisionShape* collisionShape) + { + m_collisionShape = collisionShape; + } + + int GetActivationState() const { return m_activationState1;} + + void SetActivationState(int newState); + + void ForceActivationState(int newState); + + void activate(); + + + +}; + +#endif //COLLISION_OBJECT_H diff --git a/Bullet/CollisionDispatch/CollisionWorld.cpp b/Bullet/CollisionDispatch/CollisionWorld.cpp new file mode 100644 index 000000000..1c0574129 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -0,0 +1,323 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "CollisionWorld.h" +#include "CollisionDispatcher.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionShapes/CollisionShape.h" +#include "CollisionShapes/SphereShape.h" //for raycasting +#include "CollisionShapes/TriangleMeshShape.h" //for raycasting +#include "NarrowPhaseCollision/RaycastCallback.h" + +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "BroadphaseCollision/BroadphaseInterface.h" +#include "AabbUtil2.h" + +#include + +CollisionWorld::~CollisionWorld() +{ + //clean up remaining objects + std::vector::iterator i; + + int index = 0; + for (i=m_collisionObjects.begin(); + !(i==m_collisionObjects.end()); i++) + + { + CollisionObject* collisionObject= (*i); + + BroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + GetBroadphase()->CleanProxyFromPairs(bp); + GetBroadphase()->DestroyProxy(bp); + } + } + +} + +void CollisionWorld::UpdateActivationState() +{ + m_dispatcher->InitUnionFind(m_collisionObjects.size()); + + // put the index into m_controllers into m_tag + { + std::vector::iterator i; + + int index = 0; + for (i=m_collisionObjects.begin(); + !(i==m_collisionObjects.end()); i++) + { + + CollisionObject* collisionObject= (*i); + collisionObject->m_islandTag1 = index; + collisionObject->m_hitFraction = 1.f; + index++; + + } + } + // do the union find + + m_dispatcher->FindUnions(); + + // put the islandId ('find' value) into m_tag + { + UnionFind& unionFind = m_dispatcher->GetUnionFind(); + + std::vector::iterator i; + + int index = 0; + for (i=m_collisionObjects.begin(); + !(i==m_collisionObjects.end()); i++) + { + CollisionObject* collisionObject= (*i); + + if (collisionObject->mergesSimulationIslands()) + { + collisionObject->m_islandTag1 = unionFind.find(index); + } else + { + collisionObject->m_islandTag1 = -1; + } + index++; + } + } + +} + + + + + +void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject) +{ + m_collisionObjects.push_back(collisionObject); + + //calculate new AABB + SimdTransform trans = collisionObject->m_worldTransform; + + SimdVector3 minAabb; + SimdVector3 maxAabb; + collisionObject->m_collisionShape->GetAabb(trans,minAabb,maxAabb); + + int type = collisionObject->m_collisionShape->GetShapeType(); + collisionObject->m_broadphaseHandle = GetBroadphase()->CreateProxy( + minAabb, + maxAabb, + type, + collisionObject + ); + + + + +} + +void CollisionWorld::PerformDiscreteCollisionDetection() +{ + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = 0.f; + dispatchInfo.m_stepCount = 0; + + //update aabb (of all moved objects) + + SimdVector3 aabbMin,aabbMax; + for (size_t i=0;im_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax); + m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax); + } + + m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo); +} + + +void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject) +{ + + + //bool removeFromBroadphase = false; + + { + + BroadphaseProxy* bp = collisionObject->m_broadphaseHandle; + if (bp) + { + // + // only clear the cached algorithms + // + GetBroadphase()->CleanProxyFromPairs(bp); + GetBroadphase()->DestroyProxy(bp); + } + } + + + std::vector::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 CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback) +{ + + + SimdTransform rayFromTrans,rayToTrans; + rayFromTrans.setIdentity(); + rayFromTrans.setOrigin(rayFromWorld); + rayToTrans.setIdentity(); + + rayToTrans.setOrigin(rayToWorld); + + //do culling based on aabb (rayFrom/rayTo) + SimdVector3 rayAabbMin = rayFromWorld; + SimdVector3 rayAabbMax = rayFromWorld; + rayAabbMin.setMin(rayToWorld); + rayAabbMax.setMax(rayToWorld); + + SphereShape pointShape(0.0f); + + /// brute force go over all objects. Once there is a broadphase, use that, or + /// add a raycast against aabb first. + + std::vector::iterator iter; + + for (iter=m_collisionObjects.begin(); + !(iter==m_collisionObjects.end()); iter++) + { + + CollisionObject* collisionObject= (*iter); + + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + SimdVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->m_collisionShape->GetAabb(collisionObject->m_worldTransform,collisionObjectAabbMin,collisionObjectAabbMax); + + //check aabb overlap + + if (TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,collisionObjectAabbMin,collisionObjectAabbMax)) + { + if (collisionObject->m_collisionShape->IsConvex()) + { + ConvexCast::CastResult castResult; + castResult.m_fraction = 1.f;//?? + + ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape; + VoronoiSimplexSolver simplexSolver; + SubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,collisionObject->m_worldTransform,collisionObject->m_worldTransform,castResult)) + { + //add hit + if (castResult.m_normal.length2() > 0.0001f) + { + castResult.m_normal.normalize(); + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + + + CollisionWorld::LocalRayResult localRayResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_fraction + ); + + resultCallback.AddSingleResult(localRayResult); + + } + } + } + } + else + { + + if (collisionObject->m_collisionShape->IsConcave()) + { + + TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape; + + SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse(); + + SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); + SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); + + //ConvexCast::CastResult + + struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback + { + RayResultCallback* m_resultCallback; + CollisionObject* m_collisionObject; + TriangleMeshShape* m_triangleMesh; + + BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to, + RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh): + TriangleRaycastCallback(from,to), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) + { + } + + + virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + { + CollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + + CollisionWorld::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; + + SimdVector3 rayAabbMinLocal = rayFromLocal; + rayAabbMinLocal.setMin(rayToLocal); + SimdVector3 rayAabbMaxLocal = rayFromLocal; + rayAabbMaxLocal.setMax(rayToLocal); + + triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); + + } + + + } + } + } + +} diff --git a/Bullet/CollisionDispatch/CollisionWorld.h b/Bullet/CollisionDispatch/CollisionWorld.h new file mode 100644 index 000000000..ae4101f12 --- /dev/null +++ b/Bullet/CollisionDispatch/CollisionWorld.h @@ -0,0 +1,207 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 General 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 jam, http://www.perforce.com/jam/jam.html . + * 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 CcdPhysicsEnvironment , CollisionShape and RigidBody, Stepping the simulation and synchronizing your derived version of the PHY_IMotionState class. + * @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 CollisionWorld and CollisionObject, 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 GjkPairDetector. + * + * @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 CollisionShape; +class CollisionDispatcher; +class BroadphaseInterface; +#include "SimdVector3.h" +#include "SimdTransform.h" +#include "CollisionObject.h" + +#include + +///CollisionWorld is interface and container for the collision detection +class CollisionWorld +{ + + + + std::vector m_collisionObjects; + + CollisionDispatcher* m_dispatcher; + + BroadphaseInterface* m_broadphase; + +public: + + CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase) + :m_dispatcher(dispatcher), + m_broadphase(broadphase) + { + + } + virtual ~CollisionWorld(); + + virtual void UpdateActivationState(); + + BroadphaseInterface* GetBroadphase() + { + return m_broadphase; + } + + CollisionDispatcher* GetDispatcher() + { + return m_dispatcher; + } + + ///LocalShapeInfo gives extra information for complex shapes + ///Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart + struct LocalShapeInfo + { + int m_shapePart; + int m_triangleIndex; + + //const CollisionShape* m_shapeTemp; + //const SimdTransform* m_shapeLocalTransform; + }; + + struct LocalRayResult + { + LocalRayResult(const CollisionObject* collisionObject, + LocalShapeInfo* localShapeInfo, + const SimdVector3& hitNormalLocal, + float hitFraction) + :m_collisionObject(collisionObject), + m_localShapeInfo(m_localShapeInfo), + m_hitNormalLocal(hitNormalLocal), + m_hitFraction(hitFraction) + { + } + + const CollisionObject* m_collisionObject; + LocalShapeInfo* m_localShapeInfo; + const SimdVector3& 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(const LocalRayResult& rayResult) = 0; + }; + + struct ClosestRayResultCallback : public RayResultCallback + { + ClosestRayResultCallback(SimdVector3 rayFromWorld,SimdVector3 rayToWorld) + :m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_collisionObject(0) + { + } + + SimdVector3 m_rayFromWorld;//used to calculate hitPointWorld from hitFraction + SimdVector3 m_rayToWorld; + + SimdVector3 m_hitNormalWorld; + SimdVector3 m_hitPointWorld; + const CollisionObject* m_collisionObject; + + virtual float AddSingleResult(const 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(); + } + + void RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback); + + + void AddCollisionObject(CollisionObject* collisionObject); + + void RemoveCollisionObject(CollisionObject* collisionObject); + + virtual void PerformDiscreteCollisionDetection(); + +}; + + +#endif //COLLISION_WORLD_H diff --git a/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp new file mode 100644 index 000000000..a72432ef9 --- /dev/null +++ b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.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 "ConvexConcaveCollisionAlgorithm.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionShapes/MultiSphereShape.h" +#include "CollisionShapes/BoxShape.h" +#include "ConvexConvexAlgorithm.h" +#include "BroadphaseCollision/BroadphaseProxy.h" +#include "CollisionShapes/TriangleShape.h" +#include "CollisionDispatch/ManifoldResult.h" +#include "NarrowPhaseCollision/RaycastCallback.h" +#include "CollisionShapes/TriangleMeshShape.h" + + +ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +: CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1), +m_boxTriangleCallback(ci.m_dispatcher,proxy0,proxy1) + +{ +} + +ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm() +{ +} + + + +BoxTriangleCallback::BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1): + m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher), + m_dispatchInfoPtr(0) +{ + + // + // create the manifold from the dispatcher 'manifold pool' + // + m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject); + + ClearCache(); +} + +BoxTriangleCallback::~BoxTriangleCallback() +{ + ClearCache(); + m_dispatcher->ReleaseManifold( m_manifoldPtr ); + +} + + +void BoxTriangleCallback::ClearCache() +{ + m_dispatcher->ClearManifold(m_manifoldPtr); +}; + + + +void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) +{ + + //just for debugging purposes + //printf("triangle %d",m_triangleCount++); + + + //aabb filter is already applied! + + CollisionAlgorithmConstructionInfo ci; + ci.m_dispatcher = m_dispatcher; + + + + CollisionObject* colObj = static_cast(m_boxProxy->m_clientObject); + + if (colObj->m_collisionShape->IsConvex()) + { + TriangleShape tm(triangle[0],triangle[1],triangle[2]); + tm.SetMargin(m_collisionMarginTriangle); + + CollisionObject* ob = static_cast(m_triangleProxy.m_clientObject); + + CollisionShape* tmpShape = ob->m_collisionShape; + ob->m_collisionShape = &tm; + + ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_boxProxy,&m_triangleProxy); + cvxcvxalgo.ProcessCollision(m_boxProxy,&m_triangleProxy,*m_dispatchInfoPtr); + ob->m_collisionShape = tmpShape; + + } + + + +} + + + +void BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo) +{ + m_dispatchInfoPtr = &dispatchInfo; + m_collisionMarginTriangle = collisionMarginTriangle; + + //recalc aabbs + CollisionObject* boxBody = (CollisionObject* )m_boxProxy->m_clientObject; + CollisionObject* triBody = (CollisionObject* )m_triangleProxy.m_clientObject; + + SimdTransform boxInTriangleSpace; + boxInTriangleSpace = triBody->m_worldTransform.inverse() * boxBody->m_worldTransform; + + CollisionShape* boxshape = static_cast(boxBody->m_collisionShape); + //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); + + boxshape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax); + + float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f; + + SimdVector3 extra(extraMargin,extraMargin,extraMargin); + + m_aabbMax += extra; + m_aabbMin -= extra; + +} + +void ConvexConcaveCollisionAlgorithm::ClearCache() +{ + m_boxTriangleCallback.ClearCache(); + +} + +void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo) +{ + + CollisionObject* boxBody = static_cast(m_convex.m_clientObject); + CollisionObject* triBody = static_cast(m_concave.m_clientObject); + + if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + + if (!m_dispatcher->NeedsCollision(m_convex,m_concave)) + return; + + + + CollisionObject* triOb = static_cast(m_concave.m_clientObject); + TriangleMeshShape* triangleMesh = static_cast( triOb->m_collisionShape); + + if (boxBody->m_collisionShape->IsConvex()) + { + float collisionMarginTriangle = triangleMesh->GetMargin(); + + m_boxTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo); +#ifdef USE_BOX_TRIANGLE + m_dispatcher->ClearManifold(m_boxTriangleCallback.m_manifoldPtr); +#endif + m_boxTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject); + + triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax()); + + + } + + } + +} + + +float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo) +{ + + //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the ConvexCast) + CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject; + CollisionObject* triBody = static_cast(m_concave.m_clientObject); + + const SimdVector3& from = convexbody->m_worldTransform.getOrigin(); + + SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //todo: only do if the motion exceeds the 'radius' + + struct LocalTriangleRaycastCallback : public TriangleRaycastCallback + { + LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to) + :TriangleRaycastCallback(from,to) + { + } + + virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) + { + //todo: handle ccd here + return 0.f; + + } + }; + + + LocalTriangleRaycastCallback raycastCallback(from,to); + + raycastCallback.m_hitFraction = convexbody->m_hitFraction; + + SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f); + SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY); + + if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + + CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject; + + TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->m_collisionShape; + + if (triangleMesh) + { + triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax); + } + } + + + if (raycastCallback.m_hitFraction < convexbody->m_hitFraction) + { + convexbody->m_hitFraction = raycastCallback.m_hitFraction; + return raycastCallback.m_hitFraction; + } + + return 1.f; + +} diff --git a/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h new file mode 100644 index 000000000..18652334b --- /dev/null +++ b/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BroadphaseCollision/CollisionAlgorithm.h" +#include "BroadphaseCollision/Dispatcher.h" +#include "BroadphaseCollision/BroadphaseInterface.h" +#include "CollisionShapes/TriangleCallback.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +class Dispatcher; +#include "BroadphaseCollision/BroadphaseProxy.h" + + + +class BoxTriangleCallback : public TriangleCallback +{ + BroadphaseProxy* m_boxProxy; + BroadphaseProxy m_triangleProxy; + + SimdVector3 m_aabbMin; + SimdVector3 m_aabbMax ; + + Dispatcher* m_dispatcher; + const DispatcherInfo* m_dispatchInfoPtr; + float m_collisionMarginTriangle; + +public: +int m_triangleCount; + + PersistentManifold* m_manifoldPtr; + + BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + void SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo); + + virtual ~BoxTriangleCallback(); + + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex); + + void ClearCache(); + + inline const SimdVector3& GetAabbMin() const + { + return m_aabbMin; + } + inline const SimdVector3& GetAabbMax() const + { + return m_aabbMax; + } + +}; + + + + +/// ConvexConcaveCollisionAlgorithm supports collision between convex shapes and (concave) trianges meshes. +class ConvexConcaveCollisionAlgorithm : public CollisionAlgorithm +{ + + BroadphaseProxy m_convex; + + BroadphaseProxy m_concave; + + BoxTriangleCallback m_boxTriangleCallback; + + +public: + + ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + virtual ~ConvexConcaveCollisionAlgorithm(); + + virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + void ClearCache(); + +}; + +#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp b/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp new file mode 100644 index 000000000..5654c0217 --- /dev/null +++ b/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -0,0 +1,432 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "ConvexConvexAlgorithm.h" + +#include +#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h" +#include "BroadphaseCollision/BroadphaseInterface.h" +#include "CollisionDispatch/CollisionObject.h" +#include "CollisionShapes/ConvexShape.h" +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "BroadphaseCollision/BroadphaseProxy.h" +#include "CollisionDispatch/CollisionDispatcher.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionDispatch/ManifoldResult.h" + +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" +#include "NarrowPhaseCollision/ContinuousConvexCollision.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "NarrowPhaseCollision/GjkConvexCast.h" + + + +#include "CollisionShapes/MinkowskiSumShape.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "CollisionShapes/SphereShape.h" + +#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.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; + + + +ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) +: CollisionAlgorithm(ci), +m_gjkPairDetector(0,0,&m_simplexSolver,0), +m_useEpa(!gUseEpa), +m_box0(*proxy0), +m_box1(*proxy1), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false) +{ + CheckPenetrationDepthSolver(); + + { + if (!m_manifoldPtr && m_dispatcher->NeedsCollision(m_box0,m_box1)) + { + m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject); + m_ownManifold = true; + } + } + +} + + + +ConvexConvexAlgorithm::~ConvexConvexAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->ReleaseManifold(m_manifoldPtr); + } +} + +void ConvexConvexAlgorithm ::SetLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + + +class FlippedContactResult : public DiscreteCollisionDetectorInterface::Result +{ + DiscreteCollisionDetectorInterface::Result* m_org; + +public: + + FlippedContactResult(DiscreteCollisionDetectorInterface::Result* org) + : m_org(org) + { + + } + + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) + { + SimdVector3 flippedNormal = -normalOnBInWorld; + + m_org->AddContactPoint(flippedNormal,pointInWorld,depth); + } + +}; + +static MinkowskiPenetrationDepthSolver gPenetrationDepthSolver; + +//static EpaPenetrationDepthSolver gEpaPenetrationDepthSolver; + +#ifdef USE_EPA +Solid3EpaPenetrationDepth gSolidEpaPenetrationSolver; +#endif //USE_EPA + +void ConvexConvexAlgorithm::CheckPenetrationDepthSolver() +{ + if (m_useEpa != gUseEpa) + { + m_useEpa = gUseEpa; + if (m_useEpa) + { + + // m_gjkPairDetector.SetPenetrationDepthSolver(&gEpaPenetrationDepthSolver); + + + } else + { + m_gjkPairDetector.SetPenetrationDepthSolver(&gPenetrationDepthSolver); + } + } + +} + +#ifdef USE_HULL + +Transform GetTransformFromSimdTransform(const SimdTransform& trans) +{ + //const SimdVector3& rowA0 = trans.getBasis().getRow(0); + ////const SimdVector3& rowA1 = trans.getBasis().getRow(1); + //const SimdVector3& rowA2 = trans.getBasis().getRow(2); + + SimdVector3 rowA0 = trans.getBasis().getColumn(0); + SimdVector3 rowA1 = trans.getBasis().getColumn(1); + SimdVector3 rowA2 = trans.getBasis().getColumn(2); + + + Vector3 x(rowA0.getX(),rowA0.getY(),rowA0.getZ()); + Vector3 y(rowA1.getX(),rowA1.getY(),rowA1.getZ()); + Vector3 z(rowA2.getX(),rowA2.getY(),rowA2.getZ()); + + Matrix33 ornA(x,y,z); + + Point3 transA( + trans.getOrigin().getX(), + trans.getOrigin().getY(), + trans.getOrigin().getZ()); + + return Transform(ornA,transA); +} + +class ManifoldResultCollector : public HullContactCollector +{ +public: + ManifoldResult& m_manifoldResult; + + ManifoldResultCollector(ManifoldResult& manifoldResult) + :m_manifoldResult(manifoldResult) + { + + } + + + virtual ~ManifoldResultCollector() {}; + + virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths) + { + for (int i=0;iNeedsCollision(m_box0,m_box1); + if (!needsCollision) + return; + + CollisionObject* col0 = static_cast(m_box0.m_clientObject); + CollisionObject* col1 = static_cast(m_box1.m_clientObject); + +#ifdef USE_HULL + + + if (dispatchInfo.m_enableSatConvex) + { + if ((col0->m_collisionShape->IsPolyhedral()) && + (col1->m_collisionShape->IsPolyhedral())) + { + + + PolyhedralConvexShape* polyhedron0 = static_cast(col0->m_collisionShape); + PolyhedralConvexShape* polyhedron1 = static_cast(col1->m_collisionShape); + if (polyhedron0->m_optionalHull && polyhedron1->m_optionalHull) + { + //printf("Hull-Hull"); + + //todo: cache this information, rather then initialize + Separation sep; + sep.m_featureA = 0; + sep.m_featureB = 0; + sep.m_contact = -1; + sep.m_separator = 0; + + //convert from SimdTransform to Transform + + Transform trA = GetTransformFromSimdTransform(col0->m_worldTransform); + Transform trB = GetTransformFromSimdTransform(col1->m_worldTransform); + + //either use persistent manifold or clear it every time + m_dispatcher->ClearManifold(m_manifoldPtr); + ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); + + ManifoldResultCollector hullContactCollector(*resultOut); + + Hull::ProcessHullHull(sep,*polyhedron0->m_optionalHull,*polyhedron1->m_optionalHull, + trA,trB,&hullContactCollector); + + + //user provided hull's, so we use SAT Hull collision detection + return; + } + } + } + +#endif //USE_HULL + + + ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr); + + ConvexShape* min0 = static_cast(col0->m_collisionShape); + ConvexShape* min1 = static_cast(col1->m_collisionShape); + + GjkPairDetector::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 = col0->m_worldTransform; + input.m_transformB = col1->m_worldTransform; + + m_gjkPairDetector.GetClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + m_dispatcher->ReleaseManifoldResult(resultOut); +} + + + +bool disableCcd = false; +float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) +{ + ///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; + + CollisionObject* col1 = static_cast(m_box1.m_clientObject); + CollisionObject* col0 = static_cast(m_box0.m_clientObject); + + float squareMot0 = (col0->m_interpolationWorldTransform.getOrigin() - col0->m_worldTransform.getOrigin()).length2(); + float squareMot1 = (col1->m_interpolationWorldTransform.getOrigin() - col1->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) + + bool needsCollision = m_dispatcher->NeedsCollision(m_box0,m_box1); + + if (!needsCollision) + return 1.f; + + + /// Convex0 against sphere for Convex1 + { + ConvexShape* convex0 = static_cast(col0->m_collisionShape); + + SphereShape sphere1(col1->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation + ConvexCast::CastResult result; + VoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast 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 + { + ConvexShape* convex1 = static_cast(col1->m_collisionShape); + + SphereShape sphere0(col0->m_ccdSweptShereRadius); //todo: allow non-zero sphere sizes, for better approximation + ConvexCast::CastResult result; + VoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast 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/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h b/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h new file mode 100644 index 000000000..c8ace81dd --- /dev/null +++ b/Bullet/CollisionDispatch/ConvexConvexAlgorithm.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_CONVEX_ALGORITHM_H +#define CONVEX_CONVEX_ALGORITHM_H + +#include "BroadphaseCollision/CollisionAlgorithm.h" +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "BroadphaseCollision/BroadphaseProxy.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" + +class ConvexPenetrationDepthSolver; + +///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. +class ConvexConvexAlgorithm : public CollisionAlgorithm +{ + //ConvexPenetrationDepthSolver* m_penetrationDepthSolver; + VoronoiSimplexSolver m_simplexSolver; + GjkPairDetector m_gjkPairDetector; + bool m_useEpa; +public: + BroadphaseProxy m_box0; + BroadphaseProxy m_box1; + + bool m_ownManifold; + PersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + void CheckPenetrationDepthSolver(); + + + +public: + + ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1); + + virtual ~ConvexConvexAlgorithm(); + + virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + void SetLowLevelOfDetail(bool useLowLevel); + + + + const PersistentManifold* GetManifold() + { + return m_manifoldPtr; + } + +}; + +#endif //CONVEX_CONVEX_ALGORITHM_H diff --git a/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.cpp b/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.cpp new file mode 100644 index 000000000..6ef31ad6f --- /dev/null +++ b/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.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 "EmptyCollisionAlgorithm.h" + + + +EmptyAlgorithm::EmptyAlgorithm(const CollisionAlgorithmConstructionInfo& ci) + : CollisionAlgorithm(ci) +{ +} + +void EmptyAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) +{ + +} + +float EmptyAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo) +{ + return 1.f; +} + + diff --git a/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.h b/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.h new file mode 100644 index 000000000..c85efac99 --- /dev/null +++ b/Bullet/CollisionDispatch/EmptyCollisionAlgorithm.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 EMPTY_ALGORITH +#define EMPTY_ALGORITH +#include "BroadphaseCollision/CollisionAlgorithm.h" + +#define ATTRIBUTE_ALIGNED(a) + +///EmptyAlgorithm is a stub for unsupported collision pairs. +///The dispatcher can dispatch a persistent EmptyAlgorithm to avoid a search every frame. +class EmptyAlgorithm : public CollisionAlgorithm +{ + +public: + + EmptyAlgorithm(const CollisionAlgorithmConstructionInfo& ci); + + virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo); + + + + +} ATTRIBUTE_ALIGNED(16); + +#endif //EMPTY_ALGORITH diff --git a/Bullet/CollisionDispatch/ManifoldResult.cpp b/Bullet/CollisionDispatch/ManifoldResult.cpp new file mode 100644 index 000000000..15b6512a5 --- /dev/null +++ b/Bullet/CollisionDispatch/ManifoldResult.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 "ManifoldResult.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "CollisionDispatch/CollisionObject.h" + +ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr) + :m_manifoldPtr(manifoldPtr), + m_body0(body0), + m_body1(body1) + { + } + + +void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) +{ + if (depth > m_manifoldPtr->GetContactBreakingTreshold()) + return; + + SimdTransform transAInv = m_body0->m_worldTransform.inverse(); + SimdTransform transBInv= m_body1->m_worldTransform.inverse(); + SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth; + SimdVector3 localA = transAInv(pointA ); + SimdVector3 localB = transBInv(pointInWorld); + ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + + + int insertIndex = m_manifoldPtr->GetCacheEntry(newPt); + if (insertIndex >= 0) + { + +// This is not needed, just use the old info! +// const ManifoldPoint& oldPoint = m_manifoldPtr->GetContactPoint(insertIndex); +// newPt.CopyPersistentInformation(oldPoint); +// m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex); + + + } else + { + m_manifoldPtr->AddManifoldPoint(newPt); + } +} + diff --git a/Bullet/CollisionDispatch/ManifoldResult.h b/Bullet/CollisionDispatch/ManifoldResult.h new file mode 100644 index 000000000..da3fa333a --- /dev/null +++ b/Bullet/CollisionDispatch/ManifoldResult.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 MANIFOLD_RESULT_H +#define MANIFOLD_RESULT_H + +#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h" +struct CollisionObject; +class PersistentManifold; + +///ManifoldResult is a helper class to manage contact results. +class ManifoldResult : public DiscreteCollisionDetectorInterface::Result +{ + PersistentManifold* m_manifoldPtr; + CollisionObject* m_body0; + CollisionObject* m_body1; + +public: + + ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr); + + virtual ~ManifoldResult() {}; + + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth); + +}; + +#endif //MANIFOLD_RESULT_H diff --git a/Bullet/CollisionDispatch/UnionFind.cpp b/Bullet/CollisionDispatch/UnionFind.cpp new file mode 100644 index 000000000..8f456ea17 --- /dev/null +++ b/Bullet/CollisionDispatch/UnionFind.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 "UnionFind.h" +#include + + +int UnionFind::find(int x) +{ + assert(x < m_N); + assert(x >= 0); + + while (x != m_id[x]) + { + x = m_id[x]; + assert(x < m_N); + assert(x >= 0); + + } + return x; +} + +UnionFind::~UnionFind() +{ + Free(); + +} + +UnionFind::UnionFind() +:m_id(0), +m_sz(0), +m_N(0) +{ + +} + +void UnionFind::Allocate(int N) +{ + if (m_N < N) + { + Free(); + + m_N = N; + m_id = new int[N]; + m_sz = new int[N]; + } +} +void UnionFind::Free() +{ + if (m_N) + { + m_N=0; + delete m_id; + delete m_sz; + } +} + + +void UnionFind::reset(int N) +{ + Allocate(N); + + for (int i = 0; i < m_N; i++) + { + m_id[i] = i; m_sz[i] = 1; + } +} + + +int UnionFind ::find(int p, int q) +{ + return (find(p) == find(q)); +} + +void UnionFind ::unite(int p, int q) +{ + int i = find(p), j = find(q); + if (i == j) + return; + if (m_sz[i] < m_sz[j]) + { + m_id[i] = j; m_sz[j] += m_sz[i]; + } + else + { + m_id[j] = i; m_sz[i] += m_sz[j]; + } +} diff --git a/Bullet/CollisionDispatch/UnionFind.h b/Bullet/CollisionDispatch/UnionFind.h new file mode 100644 index 000000000..f7e9feb6e --- /dev/null +++ b/Bullet/CollisionDispatch/UnionFind.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 UNION_FIND_H +#define UNION_FIND_H + +///UnionFind calculates connected subsets +class UnionFind + { + private: + int* m_id; + int* m_sz; + int m_N; + + public: + int find(int x); + + UnionFind(); + ~UnionFind(); + + void reset(int N); + + int find(int p, int q); + void unite(int p, int q); + + void Allocate(int N); + void Free(); + + }; + + +#endif //UNION_FIND_H diff --git a/Bullet/CollisionShapes/BoxShape.cpp b/Bullet/CollisionShapes/BoxShape.cpp new file mode 100644 index 000000000..2be1996da --- /dev/null +++ b/Bullet/CollisionShapes/BoxShape.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 "BoxShape.h" + +SimdVector3 BoxShape::GetHalfExtents() const +{ + return m_boxHalfExtents1 * m_localScaling; +} +//{ + + +void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const +{ + SimdVector3 halfExtents = GetHalfExtents(); + + SimdMatrix3x3 abs_b = t.getBasis().absolute(); + SimdPoint3 center = t.getOrigin(); + SimdVector3 extent = SimdVector3(abs_b[0].dot(halfExtents), + abs_b[1].dot(halfExtents), + abs_b[2].dot(halfExtents)); + extent += SimdVector3(GetMargin(),GetMargin(),GetMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; + + +} + + +void BoxShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //float margin = 0.f; + SimdVector3 halfExtents = GetHalfExtents(); + + SimdScalar lx=2.f*(halfExtents.x()); + SimdScalar ly=2.f*(halfExtents.y()); + SimdScalar 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/Bullet/CollisionShapes/BoxShape.h b/Bullet/CollisionShapes/BoxShape.h new file mode 100644 index 000000000..f47f98211 --- /dev/null +++ b/Bullet/CollisionShapes/BoxShape.h @@ -0,0 +1,279 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "PolyhedralConvexShape.h" +#include "CollisionShapes/CollisionMargin.h" +#include "BroadphaseCollision/BroadphaseProxy.h" +#include "SimdPoint3.h" +#include "SimdMinMax.h" + +///BoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box +class BoxShape: public PolyhedralConvexShape +{ + + SimdVector3 m_boxHalfExtents1; + + +public: + + virtual ~BoxShape() + { + + } + + SimdVector3 GetHalfExtents() const; + //{ return m_boxHalfExtents1 * m_localScaling;} + //const SimdVector3& GetHalfExtents() const{ return m_boxHalfExtents1;} + + + + virtual int GetShapeType() const { return BOX_SHAPE_PROXYTYPE;} + + virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const + { + + SimdVector3 halfExtents = GetHalfExtents(); + SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); + halfExtents -= margin; + + SimdVector3 supVertex; + supVertex = SimdPoint3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + + if ( GetMargin()!=0.f ) + { + SimdVector3 vecnorm = vec; + if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON)) + { + vecnorm.setValue(-1.f,-1.f,-1.f); + } + vecnorm.normalize(); + supVertex+= GetMargin() * vecnorm; + } + return supVertex; + } + + virtual inline SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const + { + SimdVector3 halfExtents = GetHalfExtents(); + SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); + halfExtents -= margin; + + return SimdVector3(vec.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(), + vec.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(), + vec.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z()); + } + + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const + { + SimdVector3 halfExtents = GetHalfExtents(); + SimdVector3 margin(GetMargin(),GetMargin(),GetMargin()); + halfExtents -= margin; + + + for (int i=0;i>1)) - halfExtents.y() * ((i&2)>>1), + halfExtents.z() * (1-((i&4)>>2)) - halfExtents.z() * ((i&4)>>2)); + } + + + virtual void GetPlaneEquation(SimdVector4& plane,int i) const + { + SimdVector3 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,SimdPoint3& pa,SimdPoint3& 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 SimdPoint3& pt,SimdScalar tolerance) const + { + SimdVector3 halfExtents = GetHalfExtents(); + + //SimdScalar 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/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp b/Bullet/CollisionShapes/BvhTriangleMeshShape.cpp new file mode 100644 index 000000000..d2624dcdf --- /dev/null +++ b/Bullet/CollisionShapes/BvhTriangleMeshShape.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 "CollisionShapes/BvhTriangleMeshShape.h" +#include "CollisionShapes/OptimizedBvh.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. +BvhTriangleMeshShape::BvhTriangleMeshShape(StridingMeshInterface* meshInterface) +:TriangleMeshShape(meshInterface) +{ + //construct bvh from meshInterface +#ifndef DISABLE_BVH + + m_bvh = new OptimizedBvh(); + m_bvh->Build(meshInterface); + +#endif //DISABLE_BVH + +} + +BvhTriangleMeshShape::~BvhTriangleMeshShape() +{ + delete m_bvh; +} + +//perform bvh tree traversal and report overlapping triangles to 'callback' +void BvhTriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const +{ + +#ifdef DISABLE_BVH + //brute force traverse all triangles + TriangleMeshShape::ProcessAllTriangles(callback,aabbMin,aabbMax); +#else + + //first get all the nodes + + + struct MyNodeOverlapCallback : public NodeOverlapCallback + { + StridingMeshInterface* m_meshInterface; + TriangleCallback* m_callback; + SimdVector3 m_triangle[3]; + + + MyNodeOverlapCallback(TriangleCallback* callback,StridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void ProcessNode(const OptimizedBvhNode* 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 SimdVector3& 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] = SimdVector3( + 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 BvhTriangleMeshShape::setLocalScaling(const SimdVector3& scaling) +{ + if ((getLocalScaling() -scaling).length2() > SIMD_EPSILON) + { + TriangleMeshShape::setLocalScaling(scaling); + delete m_bvh; + m_bvh = new OptimizedBvh(); + m_bvh->Build(m_meshInterface); + //rebuild the bvh... + } +} diff --git a/Bullet/CollisionShapes/BvhTriangleMeshShape.h b/Bullet/CollisionShapes/BvhTriangleMeshShape.h new file mode 100644 index 000000000..849ea2e99 --- /dev/null +++ b/Bullet/CollisionShapes/BvhTriangleMeshShape.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 "CollisionShapes/TriangleMeshShape.h" +#include "CollisionShapes/OptimizedBvh.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 BvhTriangleMeshShape : public TriangleMeshShape +{ + + OptimizedBvh* m_bvh; + + +public: + BvhTriangleMeshShape(StridingMeshInterface* meshInterface); + + virtual ~BvhTriangleMeshShape(); + + + /* + virtual int GetShapeType() const + { + return TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + */ + + + + virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + + //debugging + virtual char* GetName()const {return "BVHTRIANGLEMESH";} + + + virtual void setLocalScaling(const SimdVector3& scaling); + + + +}; + +#endif //BVH_TRIANGLE_MESH_SHAPE_H diff --git a/Bullet/CollisionShapes/CollisionMargin.h b/Bullet/CollisionShapes/CollisionMargin.h new file mode 100644 index 000000000..377f0e506 --- /dev/null +++ b/Bullet/CollisionShapes/CollisionMargin.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/Bullet/CollisionShapes/CollisionShape.cpp b/Bullet/CollisionShapes/CollisionShape.cpp new file mode 100644 index 000000000..0c5e48aeb --- /dev/null +++ b/Bullet/CollisionShapes/CollisionShape.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 "CollisionShapes/CollisionShape.h" + +void CollisionShape::GetBoundingSphere(SimdVector3& center,SimdScalar& radius) const +{ + SimdTransform tr; + tr.setIdentity(); + SimdVector3 aabbMin,aabbMax; + + GetAabb(tr,aabbMin,aabbMax); + + radius = (aabbMax-aabbMin).length()*0.5f; + center = (aabbMin+aabbMax)*0.5f; +} + +float CollisionShape::GetAngularMotionDisc() const +{ + SimdVector3 center; + float disc; + GetBoundingSphere(center,disc); + disc += (center).length(); + return disc; +} + +void CollisionShape::CalculateTemporalAabb(const SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep, SimdVector3& temporalAabbMin,SimdVector3& 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 + SimdVector3 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 + SimdScalar angularMotion = angvel.length() * GetAngularMotionDisc() * timeStep; + SimdVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); + temporalAabbMin = SimdVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); + temporalAabbMax = SimdVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; +} diff --git a/Bullet/CollisionShapes/CollisionShape.h b/Bullet/CollisionShapes/CollisionShape.h new file mode 100644 index 000000000..0c0f6984f --- /dev/null +++ b/Bullet/CollisionShapes/CollisionShape.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 COLLISION_SHAPE_H +#define COLLISION_SHAPE_H + +#include "SimdTransform.h" +#include "SimdVector3.h" +#include +#include "SimdPoint3.h" +#include "BroadphaseCollision/BroadphaseProxy.h" //for the shape types + +///CollisionShape provides generic interface for collidable objects +class CollisionShape +{ + +public: + + CollisionShape() + :m_tempDebug(0) + { + } + virtual ~CollisionShape() + { + } + + virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const =0; + + virtual void GetBoundingSphere(SimdVector3& center,SimdScalar& radius) const; + + 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 SimdTransform& curTrans,const SimdVector3& linvel,const SimdVector3& angvel,SimdScalar timeStep, SimdVector3& temporalAabbMin,SimdVector3& temporalAabbMax); + + bool IsPolyhedral() const + { + return (GetShapeType() < IMPLICIT_CONVEX_SHAPES_START_HERE); + } + + bool IsConvex() const + { + return (GetShapeType() < CONCAVE_SHAPES_START_HERE); + } + bool IsConcave() const + { + return (GetShapeType() > CONCAVE_SHAPES_START_HERE); + } + + + virtual void setLocalScaling(const SimdVector3& scaling) =0; + virtual const SimdVector3& getLocalScaling() const =0; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& 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/Bullet/CollisionShapes/ConeShape.cpp b/Bullet/CollisionShapes/ConeShape.cpp new file mode 100644 index 000000000..fb99c2c44 --- /dev/null +++ b/Bullet/CollisionShapes/ConeShape.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 "ConeShape.h" +#include "SimdPoint3.h" + +#ifdef WIN32 +static int coneindices[3] = {1,2,0}; +#else +static int coneindices[3] = {2,1,0}; +#endif + +ConeShape::ConeShape (SimdScalar radius,SimdScalar height): +m_radius (radius), +m_height(height) +{ + SimdVector3 halfExtents; + m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height)); +} + + +SimdVector3 ConeShape::ConeLocalSupport(const SimdVector3& v) const +{ + + float halfHeight = m_height * 0.5f; + + if (v[coneindices[1]] > v.length() * m_sinAngle) + { + SimdVector3 tmp; + + tmp[coneindices[0]] = 0.f; + tmp[coneindices[1]] = halfHeight; + tmp[coneindices[2]] = 0.f; + return tmp; + } + else { + SimdScalar s = SimdSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]); + if (s > SIMD_EPSILON) { + SimdScalar d = m_radius / s; + SimdVector3 tmp; + tmp[coneindices[0]] = v[coneindices[0]] * d; + tmp[coneindices[1]] = -halfHeight; + tmp[coneindices[2]] = v[coneindices[2]] * d; + return tmp; + } + else { + SimdVector3 tmp; + tmp[coneindices[0]] = 0.f; + tmp[coneindices[1]] = -halfHeight; + tmp[coneindices[2]] = 0.f; + return tmp; + } + } + +} + +SimdVector3 ConeShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const +{ + return ConeLocalSupport(vec); +} + +void ConeShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + return supVec; +} + +void ConvexHullShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + SimdScalar newDot; + //use 'w' component of supportVerticesOut? + { + for (int i=0;i supportVerticesOut[j][3]) + { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j] = vtx; + supportVerticesOut[j][3] = newDot; + } + } + } + + + +} + + + +SimdVector3 ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)const +{ + SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if ( GetMargin()!=0.f ) + { + SimdVector3 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 ConvexHullShape with the Raytracer Demo +int ConvexHullShape::GetNumVertices() const +{ + return m_points.size(); +} + +int ConvexHullShape::GetNumEdges() const +{ + return m_points.size()*m_points.size(); +} + +void ConvexHullShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const +{ + + int index0 = i%m_points.size(); + int index1 = i/m_points.size(); + pa = m_points[index0]*m_localScaling; + pb = m_points[index1]*m_localScaling; +} + +void ConvexHullShape::GetVertex(int i,SimdPoint3& vtx) const +{ + vtx = m_points[i]*m_localScaling; +} + +int ConvexHullShape::GetNumPlanes() const +{ + return 0; +} + +void ConvexHullShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const +{ + assert(0); +} + +//not yet +bool ConvexHullShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const +{ + assert(0); + return false; +} + diff --git a/Bullet/CollisionShapes/ConvexHullShape.h b/Bullet/CollisionShapes/ConvexHullShape.h new file mode 100644 index 000000000..cbcc5687a --- /dev/null +++ b/Bullet/CollisionShapes/ConvexHullShape.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 CONVEX_HULL_SHAPE_H +#define CONVEX_HULL_SHAPE_H + +#include "PolyhedralConvexShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +#include + +///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 ConvexHullShape : public PolyhedralConvexShape +{ + std::vector m_points; + +public: + ConvexHullShape(SimdPoint3* points,int numPoints); + + void AddPoint(const SimdPoint3& point) + { + m_points.push_back(point); + } + virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* 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,SimdPoint3& pa,SimdPoint3& pb) const; + virtual void GetVertex(int i,SimdPoint3& vtx) const; + virtual int GetNumPlanes() const; + virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const; + virtual bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const; + + + +}; + + +#endif //CONVEX_HULL_SHAPE_H + diff --git a/Bullet/CollisionShapes/ConvexShape.cpp b/Bullet/CollisionShapes/ConvexShape.cpp new file mode 100644 index 000000000..951f7a07d --- /dev/null +++ b/Bullet/CollisionShapes/ConvexShape.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 "ConvexShape.h" + +ConvexShape::~ConvexShape() +{ + +} + +ConvexShape::ConvexShape() +:m_collisionMargin(CONVEX_DISTANCE_MARGIN), +m_localScaling(1.f,1.f,1.f) +{ +} + + +void ConvexShape::setLocalScaling(const SimdVector3& scaling) +{ + m_localScaling = scaling; +} + + + +void ConvexShape::GetAabbSlow(const SimdTransform& trans,SimdVector3&minAabb,SimdVector3&maxAabb) const +{ + + SimdScalar margin = GetMargin(); + for (int i=0;i<3;i++) + { + SimdVector3 vec(0.f,0.f,0.f); + vec[i] = 1.f; + + SimdVector3 sv = LocalGetSupportingVertex(vec*trans.getBasis()); + + SimdVector3 tmp = trans(sv); + maxAabb[i] = tmp[i]+margin; + vec[i] = -1.f; + tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis())); + minAabb[i] = tmp[i]-margin; + } +}; + +SimdVector3 ConvexShape::LocalGetSupportingVertex(const SimdVector3& vec)const + { + SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec); + + if ( GetMargin()!=0.f ) + { + SimdVector3 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/Bullet/CollisionShapes/ConvexShape.h b/Bullet/CollisionShapes/ConvexShape.h new file mode 100644 index 000000000..8961f4517 --- /dev/null +++ b/Bullet/CollisionShapes/ConvexShape.h @@ -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. +*/ + +#ifndef CONVEX_SHAPE_INTERFACE1 +#define CONVEX_SHAPE_INTERFACE1 + +#include "CollisionShape.h" + +#include "SimdVector3.h" +#include "SimdTransform.h" +#include "SimdMatrix3x3.h" +#include +#include "CollisionShapes/CollisionMargin.h" + +//todo: get rid of this ConvexCastResult thing! +struct ConvexCastResult; + + +/// ConvexShape is an abstract shape interface. +/// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface. +/// used in combination with GJK or ConvexCast +class ConvexShape : public CollisionShape +{ +public: + ConvexShape(); + + virtual ~ConvexShape(); + + virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec)const; + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const= 0; + + //notice that the vectors should be unit length + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const= 0; + + // testing for hullnode code + + ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const + { + GetAabbSlow(t,aabbMin,aabbMax); + } + + + + virtual void GetAabbSlow(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + + virtual void setLocalScaling(const SimdVector3& scaling); + virtual const SimdVector3& getLocalScaling() const + { + return m_localScaling; + } + + + virtual void SetMargin(float margin) + { + m_collisionMargin = margin; + } + virtual float GetMargin() const + { + return m_collisionMargin; + } +private: + SimdScalar m_collisionMargin; + //local scaling. collisionMargin is not scaled ! +protected: + SimdVector3 m_localScaling; + +}; + + + +#endif //CONVEX_SHAPE_INTERFACE1 diff --git a/Bullet/CollisionShapes/CylinderShape.cpp b/Bullet/CollisionShapes/CylinderShape.cpp new file mode 100644 index 000000000..49163b457 --- /dev/null +++ b/Bullet/CollisionShapes/CylinderShape.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 "CylinderShape.h" +#include "SimdPoint3.h" + +CylinderShape::CylinderShape (const SimdVector3& halfExtents) +:BoxShape(halfExtents) +{ + +} + + +CylinderShapeX::CylinderShapeX (const SimdVector3& halfExtents) +:CylinderShape(halfExtents) +{ +} + + +CylinderShapeZ::CylinderShapeZ (const SimdVector3& halfExtents) +:CylinderShape(halfExtents) +{ +} + + + +inline SimdVector3 CylinderLocalSupportX(const SimdVector3& halfExtents,const SimdVector3& 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]; + + + SimdVector3 tmp; + SimdScalar d ; + + SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != SimdScalar(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] = SimdScalar(0.0); + return tmp; + } + + +} + + + + + + +inline SimdVector3 CylinderLocalSupportY(const SimdVector3& halfExtents,const SimdVector3& 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]; + + + SimdVector3 tmp; + SimdScalar d ; + + SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != SimdScalar(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] = SimdScalar(0.0); + return tmp; + } + +} + +inline SimdVector3 CylinderLocalSupportZ(const SimdVector3& halfExtents,const SimdVector3& 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]; + + + SimdVector3 tmp; + SimdScalar d ; + + SimdScalar s = SimdSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]); + if (s != SimdScalar(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] = SimdScalar(0.0); + return tmp; + } + + +} + +SimdVector3 CylinderShapeX::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const +{ + return CylinderLocalSupportX(GetHalfExtents(),vec); +} + + +SimdVector3 CylinderShapeZ::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const +{ + return CylinderLocalSupportZ(GetHalfExtents(),vec); +} +SimdVector3 CylinderShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const +{ + return CylinderLocalSupportY(GetHalfExtents(),vec); +} + +void CylinderShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i +#include "CollisionShapes/CollisionMargin.h" + + + + +/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime +class EmptyShape : public CollisionShape +{ +public: + EmptyShape(); + + virtual ~EmptyShape(); + + + ///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + + virtual void setLocalScaling(const SimdVector3& scaling) + { + m_localScaling = scaling; + } + virtual const SimdVector3& getLocalScaling() const + { + return m_localScaling; + } + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + virtual int GetShapeType() const { return EMPTY_SHAPE_PROXYTYPE;} + + virtual void SetMargin(float margin) + { + m_collisionMargin = margin; + } + virtual float GetMargin() const + { + return m_collisionMargin; + } + virtual char* GetName()const + { + return "Empty"; + } + + +private: + SimdScalar m_collisionMargin; +protected: + SimdVector3 m_localScaling; + +}; + + + +#endif //EMPTY_SHAPE_H diff --git a/Bullet/CollisionShapes/MinkowskiSumShape.cpp b/Bullet/CollisionShapes/MinkowskiSumShape.cpp new file mode 100644 index 000000000..9bd6f9c35 --- /dev/null +++ b/Bullet/CollisionShapes/MinkowskiSumShape.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 "MinkowskiSumShape.h" + + +MinkowskiSumShape::MinkowskiSumShape(ConvexShape* shapeA,ConvexShape* shapeB) +:m_shapeA(shapeA), +m_shapeB(shapeB) +{ + m_transA.setIdentity(); + m_transB.setIdentity(); +} + +SimdVector3 MinkowskiSumShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const +{ + SimdVector3 supVertexA = m_transA(m_shapeA->LocalGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + SimdVector3 supVertexB = m_transB(m_shapeB->LocalGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); + return supVertexA + supVertexB; +} + +void MinkowskiSumShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + //todo: could make recursive use of batching. probably this shape is not used frequently. + for (int i=0;iGetMargin() + m_shapeB->GetMargin(); +} + + +void MinkowskiSumShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + assert(0); + inertia.setValue(0,0,0); +} diff --git a/Bullet/CollisionShapes/MinkowskiSumShape.h b/Bullet/CollisionShapes/MinkowskiSumShape.h new file mode 100644 index 000000000..69dee7c03 --- /dev/null +++ b/Bullet/CollisionShapes/MinkowskiSumShape.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 "ConvexShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +/// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. +class MinkowskiSumShape : public ConvexShape +{ + + SimdTransform m_transA; + SimdTransform m_transB; + ConvexShape* m_shapeA; + ConvexShape* m_shapeB; + +public: + + MinkowskiSumShape(ConvexShape* shapeA,ConvexShape* shapeB); + + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + void SetTransformA(const SimdTransform& transA) { m_transA = transA;} + void SetTransformB(const SimdTransform& transB) { m_transB = transB;} + + const SimdTransform& GetTransformA()const { return m_transA;} + const SimdTransform& GetTransformB()const { return m_transB;} + + + virtual int GetShapeType() const { return MINKOWSKI_SUM_SHAPE_PROXYTYPE; } + + virtual float GetMargin() const; + + const ConvexShape* GetShapeA() const { return m_shapeA;} + const ConvexShape* GetShapeB() const { return m_shapeB;} + + virtual char* GetName()const + { + return "MinkowskiSum"; + } +}; + +#endif //MINKOWSKI_SUM_SHAPE_H diff --git a/Bullet/CollisionShapes/MultiSphereShape.cpp b/Bullet/CollisionShapes/MultiSphereShape.cpp new file mode 100644 index 000000000..83d1a72d6 --- /dev/null +++ b/Bullet/CollisionShapes/MultiSphereShape.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 "MultiSphereShape.h" +#include "CollisionShapes/CollisionMargin.h" +#include "SimdQuaternion.h" + +MultiSphereShape::MultiSphereShape (const SimdVector3& inertiaHalfExtents,const SimdVector3* positions,const SimdScalar* radi,int numSpheres) +:m_inertiaHalfExtents(inertiaHalfExtents) +{ + m_minRadius = 1e30f; + + m_numSpheres = numSpheres; + for (int i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + + void MultiSphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + + for (int j=0;j maxDot) + { + maxDot = newDot; + supportVerticesOut[j] = vtx; + } + } + } +} + + + + + + + + +void MultiSphereShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //as an approximation, take the inertia of the box that bounds the spheres + + SimdTransform ident; + ident.setIdentity(); +// SimdVector3 aabbMin,aabbMax; + +// GetAabb(ident,aabbMin,aabbMax); + + SimdVector3 halfExtents = m_inertiaHalfExtents;//(aabbMax - aabbMin)* 0.5f; + + float margin = CONVEX_DISTANCE_MARGIN; + + SimdScalar lx=2.f*(halfExtents[0]+margin); + SimdScalar ly=2.f*(halfExtents[1]+margin); + SimdScalar lz=2.f*(halfExtents[2]+margin); + const SimdScalar x2 = lx*lx; + const SimdScalar y2 = ly*ly; + const SimdScalar z2 = lz*lz; + const SimdScalar scaledmass = mass * 0.08333333f; + + inertia[0] = scaledmass * (y2+z2); + inertia[1] = scaledmass * (x2+z2); + inertia[2] = scaledmass * (x2+y2); + +} + + + diff --git a/Bullet/CollisionShapes/MultiSphereShape.h b/Bullet/CollisionShapes/MultiSphereShape.h new file mode 100644 index 000000000..19c9d978f --- /dev/null +++ b/Bullet/CollisionShapes/MultiSphereShape.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 "ConvexShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +#define MAX_NUM_SPHERES 5 + +///MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) +class MultiSphereShape : public ConvexShape + +{ + + SimdVector3 m_localPositions[MAX_NUM_SPHERES]; + SimdScalar m_radi[MAX_NUM_SPHERES]; + SimdVector3 m_inertiaHalfExtents; + + int m_numSpheres; + float m_minRadius; + + + + + +public: + MultiSphereShape (const SimdVector3& inertiaHalfExtents,const SimdVector3* positions,const SimdScalar* radi,int numSpheres); + + ///CollisionShape Interface + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + /// ConvexShape Interface + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* 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/Bullet/CollisionShapes/OptimizedBvh.cpp b/Bullet/CollisionShapes/OptimizedBvh.cpp new file mode 100644 index 000000000..0cd20ecd0 --- /dev/null +++ b/Bullet/CollisionShapes/OptimizedBvh.cpp @@ -0,0 +1,276 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "OptimizedBvh.h" +#include "StridingMeshInterface.h" +#include "AabbUtil2.h" + + + +void OptimizedBvh::Build(StridingMeshInterface* triangles) +{ + //int countTriangles = 0; + + + + // NodeArray triangleNodes; + + struct NodeTriangleCallback : public InternalTriangleIndexCallback + { + NodeArray& m_triangleNodes; + + NodeTriangleCallback(NodeArray& triangleNodes) + :m_triangleNodes(triangleNodes) + { + + } + + virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) + { + + OptimizedBvhNode node; + node.m_aabbMin = SimdVector3(1e30f,1e30f,1e30f); + node.m_aabbMax = SimdVector3(-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); + + SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f); + SimdVector3 aabbMax(1e30f,1e30f,1e30f); + + triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax); + + //now we have an array of leafnodes in m_leafNodes + + m_contiguousNodes = new OptimizedBvhNode[2*m_leafNodes.size()]; + m_curNodeIndex = 0; + + m_rootNode1 = BuildTree(m_leafNodes,0,m_leafNodes.size()); + + + ///create the leafnodes first +// OptimizedBvhNode* leafNodes = new OptimizedBvhNode; +} + + +OptimizedBvhNode* OptimizedBvh::BuildTree (NodeArray& leafNodes,int startIndex,int endIndex) +{ + OptimizedBvhNode* 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++]) OptimizedBvhNode(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;im_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 OptimizedBvh::SortAndCalcSplittingIndex(NodeArray& leafNodes,int startIndex,int endIndex,int splitAxis) +{ + int i; + int splitIndex =startIndex; + int numIndices = endIndex - startIndex; + float splitValue; + + SimdVector3 means(0.f,0.f,0.f); + for (i=startIndex;i splitValue) + { + //swap + OptimizedBvhNode tmp = leafNodes[i]; + leafNodes[i] = leafNodes[splitIndex]; + leafNodes[splitIndex] = tmp; + splitIndex++; + } + } + if ((splitIndex==startIndex) || (splitIndex == (endIndex-1))) + { + splitIndex = startIndex+ (numIndices>>1); + } + return splitIndex; +} + + +int OptimizedBvh::CalcSplittingAxis(NodeArray& leafNodes,int startIndex,int endIndex) +{ + int i; + + SimdVector3 means(0.f,0.f,0.f); + SimdVector3 variance(0.f,0.f,0.f); + int numIndices = endIndex-startIndex; + + for (i=startIndex;i 1000.f) + { + for (size_t i=0;iProcessNode(&node); + } + } else + { + //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax); + } +} + +void OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& 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 OptimizedBvh::WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& 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 OptimizedBvh::ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const +{ + +} + diff --git a/Bullet/CollisionShapes/OptimizedBvh.h b/Bullet/CollisionShapes/OptimizedBvh.h new file mode 100644 index 000000000..ec550e467 --- /dev/null +++ b/Bullet/CollisionShapes/OptimizedBvh.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 "SimdVector3.h" +#include + +class StridingMeshInterface; + +/// OptimizedBvhNode 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 OptimizedBvhNode +{ + + SimdVector3 m_aabbMin; + SimdVector3 m_aabbMax; + +//these 2 pointers are obsolete, the stackless traversal just uses the escape index + OptimizedBvhNode* m_leftChild; + OptimizedBvhNode* m_rightChild; + + int m_escapeIndex; + + //for child nodes + int m_subPart; + int m_triangleIndex; + +}; + +class NodeOverlapCallback +{ +public: + virtual ~NodeOverlapCallback() {}; + + virtual void ProcessNode(const OptimizedBvhNode* node) = 0; +}; + +typedef std::vector NodeArray; + + +///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future) +class OptimizedBvh +{ + OptimizedBvhNode* m_rootNode1; + + OptimizedBvhNode* m_contiguousNodes; + int m_curNodeIndex; + + int m_numNodes; + + NodeArray m_leafNodes; + +public: + OptimizedBvh() :m_rootNode1(0), m_numNodes(0) { } + virtual ~OptimizedBvh() {}; + + void Build(StridingMeshInterface* triangles); + + OptimizedBvhNode* 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(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + void WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + + //OptimizedBvhNode* GetRootNode() { return m_rootNode1;} + + int GetNumNodes() { return m_numNodes;} + + void ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + void ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + +}; + + +#endif //OPTIMIZED_BVH_H + diff --git a/Bullet/CollisionShapes/PolyhedralConvexShape.cpp b/Bullet/CollisionShapes/PolyhedralConvexShape.cpp new file mode 100644 index 000000000..bfdf75014 --- /dev/null +++ b/Bullet/CollisionShapes/PolyhedralConvexShape.cpp @@ -0,0 +1,113 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 + +PolyhedralConvexShape::PolyhedralConvexShape() +:m_optionalHull(0) +{ + +} + + + +SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const +{ + int i; + SimdVector3 supVec(0,0,0); + + SimdScalar maxDot(-1e30f); + + SimdVector3 vec = vec0; + SimdScalar lenSqr = vec.length2(); + if (lenSqr < 0.0001f) + { + vec.setValue(1,0,0); + } else + { + float rlen = 1.f / SimdSqrt(lenSqr ); + vec *= rlen; + } + + SimdVector3 vtx; + SimdScalar newDot; + + for (i=0;i maxDot) + { + maxDot = newDot; + supVec = vtx; + } + } + + return supVec; + +} + +void PolyhedralConvexShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + int i; + + SimdVector3 vtx; + SimdScalar newDot; + + for (int j=0;j maxDot) + { + maxDot = newDot; + supportVerticesOut[i] = vtx; + } + } + } +} + + + +void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //not yet, return box inertia + + float margin = GetMargin(); + + SimdTransform ident; + ident.setIdentity(); + SimdVector3 aabbMin,aabbMax; + GetAabb(ident,aabbMin,aabbMax); + SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f; + + SimdScalar lx=2.f*(halfExtents.x()+margin); + SimdScalar ly=2.f*(halfExtents.y()+margin); + SimdScalar lz=2.f*(halfExtents.z()+margin); + const SimdScalar x2 = lx*lx; + const SimdScalar y2 = ly*ly; + const SimdScalar z2 = lz*lz; + const SimdScalar scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2)); + +} + diff --git a/Bullet/CollisionShapes/PolyhedralConvexShape.h b/Bullet/CollisionShapes/PolyhedralConvexShape.h new file mode 100644 index 000000000..358bf71d0 --- /dev/null +++ b/Bullet/CollisionShapes/PolyhedralConvexShape.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 +#include +#include + + +///PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. +class PolyhedralConvexShape : public ConvexShape +{ + +public: + + PolyhedralConvexShape(); + + //brute force implementations + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const; + virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + + + virtual int GetNumVertices() const = 0 ; + virtual int GetNumEdges() const = 0; + virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const = 0; + virtual void GetVertex(int i,SimdPoint3& vtx) const = 0; + virtual int GetNumPlanes() const = 0; + virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const = 0; +// virtual int GetIndex(int i) const = 0 ; + + virtual bool IsInside(const SimdPoint3& pt,SimdScalar 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/Bullet/CollisionShapes/Simplex1to4Shape.cpp b/Bullet/CollisionShapes/Simplex1to4Shape.cpp new file mode 100644 index 000000000..e1d02f956 --- /dev/null +++ b/Bullet/CollisionShapes/Simplex1to4Shape.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 "Simplex1to4Shape.h" +#include "SimdMatrix3x3.h" + +BU_Simplex1to4::BU_Simplex1to4() +:m_numVertices(0) +{ +} + +BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0) +:m_numVertices(0) +{ + AddVertex(pt0); +} + +BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1) +:m_numVertices(0) +{ + AddVertex(pt0); + AddVertex(pt1); +} + +BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2) +:m_numVertices(0) +{ + AddVertex(pt0); + AddVertex(pt1); + AddVertex(pt2); +} + +BU_Simplex1to4::BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2,const SimdPoint3& pt3) +:m_numVertices(0) +{ + AddVertex(pt0); + AddVertex(pt1); + AddVertex(pt2); + AddVertex(pt3); +} + + + + + +void BU_Simplex1to4::AddVertex(const SimdPoint3& pt) +{ + m_vertices[m_numVertices++] = pt; +} + + +int BU_Simplex1to4::GetNumVertices() const +{ + return m_numVertices; +} + +int BU_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 BU_Simplex1to4::GetEdge(int i,SimdPoint3& pa,SimdPoint3& 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 BU_Simplex1to4::GetVertex(int i,SimdPoint3& vtx) const +{ + vtx = m_vertices[i]; +} + +int BU_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 BU_Simplex1to4::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i) const +{ + +} + +int BU_Simplex1to4::GetIndex(int i) const +{ + return 0; +} + +bool BU_Simplex1to4::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const +{ + return false; +} + diff --git a/Bullet/CollisionShapes/Simplex1to4Shape.h b/Bullet/CollisionShapes/Simplex1to4Shape.h new file mode 100644 index 000000000..8e5d124f3 --- /dev/null +++ b/Bullet/CollisionShapes/Simplex1to4Shape.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 +#include "BroadphaseCollision/BroadphaseProxy.h" + + +///BU_Simplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex). +class BU_Simplex1to4 : public PolyhedralConvexShape +{ +protected: + + int m_numVertices; + SimdPoint3 m_vertices[4]; + +public: + BU_Simplex1to4(); + + BU_Simplex1to4(const SimdPoint3& pt0); + BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1); + BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2); + BU_Simplex1to4(const SimdPoint3& pt0,const SimdPoint3& pt1,const SimdPoint3& pt2,const SimdPoint3& pt3); + + + void Reset() + { + m_numVertices = 0; + } + + + virtual int GetShapeType() const{ return TETRAHEDRAL_SHAPE_PROXYTYPE; } + + void AddVertex(const SimdPoint3& pt); + + //PolyhedralConvexShape interface + + virtual int GetNumVertices() const; + + virtual int GetNumEdges() const; + + virtual void GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const; + + virtual void GetVertex(int i,SimdPoint3& vtx) const; + + virtual int GetNumPlanes() const; + + virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i) const; + + virtual int GetIndex(int i) const; + + virtual bool IsInside(const SimdPoint3& pt,SimdScalar tolerance) const; + + + ///GetName is for debugging + virtual char* GetName()const { return "BU_Simplex1to4";} + +}; + +#endif //BU_SIMPLEX_1TO4_SHAPE diff --git a/Bullet/CollisionShapes/SphereShape.cpp b/Bullet/CollisionShapes/SphereShape.cpp new file mode 100644 index 000000000..28c46a28a --- /dev/null +++ b/Bullet/CollisionShapes/SphereShape.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 "SphereShape.h" +#include "CollisionShapes/CollisionMargin.h" + +#include "SimdQuaternion.h" + + +SphereShape ::SphereShape (SimdScalar radius) +: m_radius(radius) +{ +} + +SimdVector3 SphereShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const +{ + return SimdVector3(0.f,0.f,0.f); +} + +void SphereShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const +{ + for (int i=0;i=0;j--) + { + + graphicsindex = gfxbase[j]; +#ifdef DEBUG_TRIANGLE_MESH + printf("%d ,",graphicsindex); +#endif //DEBUG_TRIANGLE_MESH + float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + + triangle[j] = SimdVector3( + 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 + } + + + //check aabb in triangle-space, before doing this + callback->InternalProcessTriangleIndex(triangle,part,gfxindex); + + } + + unLockReadOnlyVertexBase(part); + } +} + diff --git a/Bullet/CollisionShapes/StridingMeshInterface.h b/Bullet/CollisionShapes/StridingMeshInterface.h new file mode 100644 index 000000000..0b482afdc --- /dev/null +++ b/Bullet/CollisionShapes/StridingMeshInterface.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 "SimdVector3.h" +#include "TriangleCallback.h" + +/// PHY_ScalarType enumerates possible scalar types. +/// See the StridingMeshInterface for its use +typedef enum PHY_ScalarType { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88 +} PHY_ScalarType; + +/// StridingMeshInterface 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 StridingMeshInterface +{ + protected: + + SimdVector3 m_scaling; + + public: + StridingMeshInterface() :m_scaling(1.f,1.f,1.f) + { + + } + + virtual ~StridingMeshInterface(); + + + + void InternalProcessAllTriangles(InternalTriangleIndexCallback* callback,const SimdVector3& aabbMin,const SimdVector3& 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 SimdVector3& getScaling() const { + return m_scaling; + } + void setScaling(const SimdVector3& scaling) + { + m_scaling = scaling; + } + + +}; + +#endif //STRIDING_MESHINTERFACE_H diff --git a/Bullet/CollisionShapes/TriangleCallback.cpp b/Bullet/CollisionShapes/TriangleCallback.cpp new file mode 100644 index 000000000..499d377a0 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleCallback.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 "TriangleCallback.h" + +TriangleCallback::~TriangleCallback() +{ + +} + + +InternalTriangleIndexCallback::~InternalTriangleIndexCallback() +{ + +} + diff --git a/Bullet/CollisionShapes/TriangleCallback.h b/Bullet/CollisionShapes/TriangleCallback.h new file mode 100644 index 000000000..810d4e626 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleCallback.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 "SimdVector3.h" + + +class TriangleCallback +{ +public: + + virtual ~TriangleCallback(); + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex) = 0; +}; + +class InternalTriangleIndexCallback +{ +public: + + virtual ~InternalTriangleIndexCallback(); + virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int triangleIndex) = 0; +}; + + + +#endif //TRIANGLE_CALLBACK_H diff --git a/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp b/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp new file mode 100644 index 000000000..ebbb7c3e5 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleIndexVertexArray.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 "TriangleIndexVertexArray.h" + +TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride) +:m_numTriangleIndices(numTriangleIndices), +m_triangleIndexBase(triangleIndexBase), +m_triangleIndexStride(triangleIndexStride), +m_numVertices(numVertices), +m_vertexBase(vertexBase), +m_vertexStride(vertexStride) +{ +} + +void TriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart) +{ + numverts = m_numVertices; + (*vertexbase) = (unsigned char *)m_vertexBase; + type = PHY_FLOAT; + vertexStride = m_vertexStride; + + numfaces = m_numTriangleIndices; + (*indexbase) = (unsigned char *)m_triangleIndexBase; + indexstride = m_triangleIndexStride; + indicestype = PHY_INTEGER; +} + +void TriangleIndexVertexArray::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 +{ + numverts = m_numVertices; + (*vertexbase) = (unsigned char *)m_vertexBase; + type = PHY_FLOAT; + vertexStride = m_vertexStride; + + numfaces = m_numTriangleIndices; + (*indexbase) = (unsigned char *)m_triangleIndexBase; + indexstride = m_triangleIndexStride; + indicestype = PHY_INTEGER; +} + diff --git a/Bullet/CollisionShapes/TriangleIndexVertexArray.h b/Bullet/CollisionShapes/TriangleIndexVertexArray.h new file mode 100644 index 000000000..1134521ad --- /dev/null +++ b/Bullet/CollisionShapes/TriangleIndexVertexArray.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. +*/ + +#include "StridingMeshInterface.h" + + +class TriangleIndexVertexArray : public StridingMeshInterface +{ + + int m_numTriangleIndices; + int* m_triangleIndexBase; + int m_triangleIndexStride; + int m_numVertices; + float* m_vertexBase; + int m_vertexStride; + +public: + + TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride); + + 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 1;} + + virtual void preallocateVertices(int numverts){} + virtual void preallocateIndices(int numindices){} + +}; + diff --git a/Bullet/CollisionShapes/TriangleMesh.cpp b/Bullet/CollisionShapes/TriangleMesh.cpp new file mode 100644 index 000000000..c86d286d9 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleMesh.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 "TriangleMesh.h" +#include + +static int myindices[3] = {0,1,2}; + +TriangleMesh::TriangleMesh () +{ + +} + +void TriangleMesh::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(SimdVector3); + + + numfaces = 1; + *indexbase = (unsigned char*) &myindices[0]; + indicestype = PHY_INTEGER; + indexstride = sizeof(int); + +} + +void TriangleMesh::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(SimdVector3); + + + numfaces = 1; + *indexbase = (unsigned char*) &myindices[0]; + indicestype = PHY_INTEGER; + indexstride = sizeof(int); + +} + + + +int TriangleMesh::getNumSubParts() const +{ + return m_triangles.size(); +} diff --git a/Bullet/CollisionShapes/TriangleMesh.h b/Bullet/CollisionShapes/TriangleMesh.h new file mode 100644 index 000000000..9623cb401 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleMesh.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 "CollisionShapes/StridingMeshInterface.h" +#include +#include + +struct MyTriangle +{ + SimdVector3 m_vert0; + SimdVector3 m_vert1; + SimdVector3 m_vert2; +}; + +///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the TriangleMeshShape. +class TriangleMesh : public StridingMeshInterface +{ + std::vector m_triangles; + + + public: + TriangleMesh (); + + void AddTriangle(const SimdVector3& vertex0,const SimdVector3& vertex1,const SimdVector3& vertex2) + { + MyTriangle 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/Bullet/CollisionShapes/TriangleMeshShape.cpp b/Bullet/CollisionShapes/TriangleMeshShape.cpp new file mode 100644 index 000000000..73b53cd02 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleMeshShape.cpp @@ -0,0 +1,202 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "TriangleMeshShape.h" +#include "SimdVector3.h" +#include "SimdQuaternion.h" +#include "StridingMeshInterface.h" +#include "AabbUtil2.h" +#include "CollisionShapes/CollisionMargin.h" + +#include "stdio.h" + +TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface) +: m_meshInterface(meshInterface), +m_collisionMargin(CONVEX_DISTANCE_MARGIN) +{ + RecalcLocalAabb(); +} + + +TriangleMeshShape::~TriangleMeshShape() +{ + +} + + + + +void TriangleMeshShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const +{ + + SimdVector3 localHalfExtents = 0.5f*(m_localAabbMax-m_localAabbMin); + SimdVector3 localCenter = 0.5f*(m_localAabbMax+m_localAabbMin); + + SimdMatrix3x3 abs_b = trans.getBasis().absolute(); + + SimdPoint3 center = trans(localCenter); + + SimdVector3 extent = SimdVector3(abs_b[0].dot(localHalfExtents), + abs_b[1].dot(localHalfExtents), + abs_b[2].dot(localHalfExtents)); + extent += SimdVector3(GetMargin(),GetMargin(),GetMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; + + +} + +void TriangleMeshShape::RecalcLocalAabb() +{ + for (int i=0;i<3;i++) + { + SimdVector3 vec(0.f,0.f,0.f); + vec[i] = 1.f; + SimdVector3 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 TriangleCallback +{ + + SimdVector3 m_supportVertexLocal; +public: + + SimdTransform m_worldTrans; + SimdScalar m_maxDot; + SimdVector3 m_supportVecLocal; + + SupportVertexCallback(const SimdVector3& supportVecWorld,const SimdTransform& trans) + : m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f) + + { + m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis(); + } + + virtual void ProcessTriangle( SimdVector3* triangle,int partId, int triangleIndex) + { + for (int i=0;i<3;i++) + { + SimdScalar dot = m_supportVecLocal.dot(triangle[i]); + if (dot > m_maxDot) + { + m_maxDot = dot; + m_supportVertexLocal = triangle[i]; + } + } + } + + SimdVector3 GetSupportVertexWorldSpace() + { + return m_worldTrans(m_supportVertexLocal); + } + + SimdVector3 GetSupportVertexLocal() + { + return m_supportVertexLocal; + } + +}; + + +void TriangleMeshShape::setLocalScaling(const SimdVector3& scaling) +{ + m_meshInterface->setScaling(scaling); + RecalcLocalAabb(); +} + +const SimdVector3& TriangleMeshShape::getLocalScaling() const +{ + return m_meshInterface->getScaling(); +} + + + + + + +//#define DEBUG_TRIANGLE_MESH + + +void TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const +{ + + struct FilteredCallback : public InternalTriangleIndexCallback + { + TriangleCallback* m_callback; + SimdVector3 m_aabbMin; + SimdVector3 m_aabbMax; + + FilteredCallback(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) + :m_callback(callback), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) + { + } + + virtual void InternalProcessTriangleIndex(SimdVector3* 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 TriangleMeshShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + //moving concave objects not supported + assert(0); + inertia.setValue(0.f,0.f,0.f); +} + + +SimdVector3 TriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec) const +{ + SimdVector3 supportVertex; + + SimdTransform ident; + ident.setIdentity(); + + SupportVertexCallback supportCallback(vec,ident); + + SimdVector3 aabbMax(1e30f,1e30f,1e30f); + + ProcessAllTriangles(&supportCallback,-aabbMax,aabbMax); + + supportVertex = supportCallback.GetSupportVertexLocal(); + + return supportVertex; +} diff --git a/Bullet/CollisionShapes/TriangleMeshShape.h b/Bullet/CollisionShapes/TriangleMeshShape.h new file mode 100644 index 000000000..ccbb4d75f --- /dev/null +++ b/Bullet/CollisionShapes/TriangleMeshShape.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 TRIANGLE_MESH_SHAPE_H +#define TRIANGLE_MESH_SHAPE_H + +#include "CollisionShapes/CollisionShape.h" +#include "BroadphaseCollision/BroadphaseProxy.h" // for the types + +#include "StridingMeshInterface.h" +#include "TriangleCallback.h" + + + +///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles. +class TriangleMeshShape : public CollisionShape +{ +protected: + StridingMeshInterface* m_meshInterface; + SimdVector3 m_localAabbMin; + SimdVector3 m_localAabbMax; + float m_collisionMargin; + +public: + TriangleMeshShape(StridingMeshInterface* meshInterface); + + virtual ~TriangleMeshShape(); + + + + virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const; + + virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const + { + assert(0); + return LocalGetSupportingVertex(vec); + } + + void RecalcLocalAabb(); + + virtual int GetShapeType() const + { + return TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + + virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const; + + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); + + virtual void setLocalScaling(const SimdVector3& scaling); + virtual const SimdVector3& getLocalScaling() const; + + + //debugging + virtual char* GetName()const {return "TRIANGLEMESH";} + + + virtual float GetMargin() const { + return m_collisionMargin; + } + virtual void SetMargin(float collisionMargin) + { + m_collisionMargin = collisionMargin; + } + + + +}; + +#endif //TRIANGLE_MESH_SHAPE_H diff --git a/Bullet/CollisionShapes/TriangleShape.h b/Bullet/CollisionShapes/TriangleShape.h new file mode 100644 index 000000000..d1b4a9930 --- /dev/null +++ b/Bullet/CollisionShapes/TriangleShape.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 "ConvexShape.h" +#include "CollisionShapes/BoxShape.h" + +class TriangleShape : public PolyhedralConvexShape +{ + + +public: + + SimdVector3 m_vertices1[3]; + + + virtual int GetNumVertices() const + { + return 3; + } + + const SimdVector3& GetVertexPtr(int index) const + { + return m_vertices1[index]; + } + virtual void GetVertex(int index,SimdVector3& 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,SimdPoint3& pa,SimdPoint3& pb) const + { + GetVertex(i,pa); + GetVertex((i+1)%3,pb); + } + + virtual void GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax)const + { +// ASSERT(0); + GetAabbSlow(t,aabbMin,aabbMax); + } + + SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& dir)const + { + SimdVector3 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 SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const + { + for (int i=0;i= -tolerance && dist <= tolerance) + { + //inside check on edge-planes + int i; + for (i=0;i<3;i++) + { + SimdPoint3 pa,pb; + GetEdge(i,pa,pb); + SimdVector3 edge = pb-pa; + SimdVector3 edgeNormal = edge.cross(normal); + edgeNormal.normalize(); + SimdScalar dist = pt.dot( edgeNormal); + SimdScalar 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/Bullet/Doxyfile b/Bullet/Doxyfile new file mode 100644 index 000000000..4ecb6acb6 --- /dev/null +++ b/Bullet/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 , where +# is the value of the INPUT_FILTER tag, and 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/Bullet/Jamfile b/Bullet/Jamfile new file mode 100644 index 000000000..a2d3b9676 --- /dev/null +++ b/Bullet/Jamfile @@ -0,0 +1,11 @@ +SubDir TOP Bullet ; + + +Description bullet : "Bullet Continuous Collision Detection and Physics Library" ; +Library bullet : + [ Wildcard BroadphaseCollision : *.h *.cpp ] + [ Wildcard CollisionDispatch : *.h *.cpp ] + [ Wildcard CollisionShapes : *.h *.cpp ] + [ Wildcard NarrowPhaseCollision : *.h *.cpp ] +; +Recurse InstallHeader : .h ; diff --git a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp b/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp new file mode 100644 index 000000000..e318c25da --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.cpp @@ -0,0 +1,360 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_AlgebraicPolynomialSolver.h" +#include +#include + +int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) +{ + + SimdScalar basic_h_local; + SimdScalar basic_h_local_delta; + + basic_h_local = p * 0.5f; + basic_h_local_delta = basic_h_local * basic_h_local - q; + if (basic_h_local_delta > 0.0f) { + basic_h_local_delta = SimdSqrt(basic_h_local_delta); + m_roots[0] = - basic_h_local + basic_h_local_delta; + m_roots[1] = - basic_h_local - basic_h_local_delta; + return 2; + } + else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { + m_roots[0] = - basic_h_local; + return 1; + } + else { + return 0; + } + } + + +int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) +{ + SimdScalar radical = b * b - 4.0f * a * c; + if(radical >= 0.f) + { + SimdScalar sqrtRadical = SimdSqrt(radical); + SimdScalar idenom = 1.0f/(2.0f * a); + m_roots[0]=(-b + sqrtRadical) * idenom; + m_roots[1]=(-b - sqrtRadical) * idenom; + return 2; + } + return 0; +} + + +#define cubic_rt(x) \ + ((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \ + ((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f)) + + + +/* */ +/* this function solves the following cubic equation: */ +/* */ +/* 3 2 */ +/* lead * x + a * x + b * x + c = 0. */ +/* */ +/* it returns the number of different roots found, and stores the roots in */ +/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */ +/* */ +int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) +{ + SimdScalar p, q, r; + SimdScalar delta, u, phi; + SimdScalar dummy; + + if (lead != 1.0) { + /* */ + /* transform into normal form: x^3 + a x^2 + b x + c = 0 */ + /* */ + if (SimdEqual(lead, SIMD_EPSILON)) { + /* */ + /* we have a x^2 + b x + c = 0 */ + /* */ + if (SimdEqual(a, SIMD_EPSILON)) { + /* */ + /* we have b x + c = 0 */ + /* */ + if (SimdEqual(b, SIMD_EPSILON)) { + if (SimdEqual(c, SIMD_EPSILON)) { + return -1; + } + else { + return 0; + } + } + else { + m_roots[0] = -c / b; + return 1; + } + } + else { + p = c / a; + q = b / a; + return Solve2QuadraticFull(a,b,c); + } + } + else { + a = a / lead; + b = b / lead; + c = c / lead; + } + } + + /* */ + /* we substitute x = y - a / 3 in order to eliminate the quadric term. */ + /* we get x^3 + p x + q = 0 */ + /* */ + a /= 3.0f; + u = a * a; + p = b / 3.0f - u; + q = a * (2.0f * u - b) + c; + + /* */ + /* now use Cardano's formula */ + /* */ + if (SimdEqual(p, SIMD_EPSILON)) { + if (SimdEqual(q, SIMD_EPSILON)) { + /* */ + /* one triple root */ + /* */ + m_roots[0] = -a; + return 1; + } + else { + /* */ + /* one real and two complex roots */ + /* */ + m_roots[0] = cubic_rt(-q) - a; + return 1; + } + } + + q /= 2.0f; + delta = p * p * p + q * q; + if (delta > 0.0f) { + /* */ + /* one real and two complex roots. note that v = -p / u. */ + /* */ + u = -q + SimdSqrt(delta); + u = cubic_rt(u); + m_roots[0] = u - p / u - a; + return 1; + } + else if (delta < 0.0) { + /* */ + /* Casus irreducibilis: we have three real roots */ + /* */ + r = SimdSqrt(-p); + p *= -r; + r *= 2.0; + phi = SimdAcos(-q / p) / 3.0f; + dummy = SIMD_2_PI / 3.0f; + m_roots[0] = r * SimdCos(phi) - a; + m_roots[1] = r * SimdCos(phi + dummy) - a; + m_roots[2] = r * SimdCos(phi - dummy) - a; + return 3; + } + else { + /* */ + /* one single and one SimdScalar root */ + /* */ + r = cubic_rt(-q); + m_roots[0] = 2.0f * r - a; + m_roots[1] = -r - a; + return 2; + } +} + + +/* */ +/* this function solves the following quartic equation: */ +/* */ +/* 4 3 2 */ +/* lead * x + a * x + b * x + c * x + d = 0. */ +/* */ +/* it returns the number of different roots found, and stores the roots in */ +/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */ +/* */ +int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) +{ + SimdScalar p, q ,r; + SimdScalar u, v, w; + int i, num_roots, num_tmp; + //SimdScalar tmp[2]; + + if (lead != 1.0) { + /* */ + /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */ + /* */ + if (SimdEqual(lead, SIMD_EPSILON)) { + /* */ + /* we have a x^3 + b x^2 + c x + d = 0 */ + /* */ + if (SimdEqual(a, SIMD_EPSILON)) { + /* */ + /* we have b x^2 + c x + d = 0 */ + /* */ + if (SimdEqual(b, SIMD_EPSILON)) { + /* */ + /* we have c x + d = 0 */ + /* */ + if (SimdEqual(c, SIMD_EPSILON)) { + if (SimdEqual(d, SIMD_EPSILON)) { + return -1; + } + else { + return 0; + } + } + else { + m_roots[0] = -d / c; + return 1; + } + } + else { + p = c / b; + q = d / b; + return Solve2QuadraticFull(b,c,d); + + } + } + else { + return Solve3Cubic(1.0, b / a, c / a, d / a); + } + } + else { + a = a / lead; + b = b / lead; + c = c / lead; + d = d / lead; + } + } + + /* */ + /* we substitute x = y - a / 4 in order to eliminate the cubic term. */ + /* we get: y^4 + p y^2 + q y + r = 0. */ + /* */ + a /= 4.0f; + p = b - 6.0f * a * a; + q = a * (8.0f * a * a - 2.0f * b) + c; + r = a * (a * (b - 3.f * a * a) - c) + d; + if (SimdEqual(q, SIMD_EPSILON)) { + /* */ + /* biquadratic equation: y^4 + p y^2 + r = 0. */ + /* */ + num_roots = Solve2Quadratic(p, r); + if (num_roots > 0) { + if (m_roots[0] > 0.0f) { + if (num_roots > 1) { + if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) { + u = SimdSqrt(m_roots[1]); + m_roots[2] = u - a; + m_roots[3] = -u - a; + u = SimdSqrt(m_roots[0]); + m_roots[0] = u - a; + m_roots[1] = -u - a; + return 4; + } + else { + u = SimdSqrt(m_roots[0]); + m_roots[0] = u - a; + m_roots[1] = -u - a; + return 2; + } + } + else { + u = SimdSqrt(m_roots[0]); + m_roots[0] = u - a; + m_roots[1] = -u - a; + return 2; + } + } + } + return 0; + } + else if (SimdEqual(r, SIMD_EPSILON)) { + /* */ + /* no absolute term: y (y^3 + p y + q) = 0. */ + /* */ + num_roots = Solve3Cubic(1.0, 0.0, p, q); + for (i = 0; i < num_roots; ++i) m_roots[i] -= a; + if (num_roots != -1) { + m_roots[num_roots] = -a; + ++num_roots; + } + else { + m_roots[0] = -a; + num_roots = 1;; + } + return num_roots; + } + else { + /* */ + /* we solve the resolvent cubic equation */ + /* */ + num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q); + if (num_roots == -1) { + num_roots = 1; + m_roots[0] = 0.0f; + } + + /* */ + /* build two quadric equations */ + /* */ + w = m_roots[0]; + u = w * w - r; + v = 2.0f * w - p; + + if (SimdEqual(u, SIMD_EPSILON)) + u = 0.0; + else if (u > 0.0f) + u = SimdSqrt(u); + else + return 0; + + if (SimdEqual(v, SIMD_EPSILON)) + v = 0.0; + else if (v > 0.0f) + v = SimdSqrt(v); + else + return 0; + + if (q < 0.0f) v = -v; + w -= u; + num_roots=Solve2Quadratic(v, w); + for (i = 0; i < num_roots; ++i) + { + m_roots[i] -= a; + } + w += 2.0f *u; + SimdScalar tmp[2]; + tmp[0] = m_roots[0]; + tmp[1] = m_roots[1]; + + num_tmp = Solve2Quadratic(-v, w); + for (i = 0; i < num_tmp; ++i) + { + m_roots[i + num_roots] = tmp[i] - a; + m_roots[i]=tmp[i]; + } + + return (num_tmp + num_roots); + } +} + diff --git a/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h b/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h new file mode 100644 index 000000000..2d2fe5fc1 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_AlgebraicPolynomialSolver.h @@ -0,0 +1,45 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_ALGEBRAIC_POLYNOMIAL_SOLVER_H +#define BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H + +#include "BU_PolynomialSolverInterface.h" + +/// BU_AlgebraicPolynomialSolver implements polynomial root finding by analytically solving algebraic equations. +/// Polynomials up to 4rd degree are supported, Cardano's formula is used for 3rd degree +class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface +{ +public: + BU_AlgebraicPolynomialSolver() {}; + + int Solve2Quadratic(SimdScalar p, SimdScalar q); + int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c); + int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c); + int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d); + + + SimdScalar GetRoot(int i) const + { + return m_roots[i]; + } + +private: + SimdScalar m_roots[4]; + +}; + +#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H diff --git a/Bullet/NarrowPhaseCollision/BU_Collidable.cpp b/Bullet/NarrowPhaseCollision/BU_Collidable.cpp new file mode 100644 index 000000000..bb06e7edb --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_Collidable.cpp @@ -0,0 +1,25 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_Collidable.h" +#include "CollisionShapes/CollisionShape.h" +#include +#include "BU_MotionStateInterface.h" + +BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer ) +:m_motionState(motion),m_shape(shape),m_userPointer(userPointer) +{ +} diff --git a/Bullet/NarrowPhaseCollision/BU_Collidable.h b/Bullet/NarrowPhaseCollision/BU_Collidable.h new file mode 100644 index 000000000..0a63d32f6 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_Collidable.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 BU_COLLIDABLE +#define BU_COLLIDABLE + + +class PolyhedralConvexShape; +class BU_MotionStateInterface; +#include + +class BU_Collidable +{ +public: + BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer); + + void* GetUserPointer() const + { + return m_userPointer; + } + + BU_MotionStateInterface& GetMotionState() + { + return m_motionState; + } + inline const BU_MotionStateInterface& GetMotionState() const + { + return m_motionState; + } + + inline const PolyhedralConvexShape& GetShape() const + { + return m_shape; + }; + + +private: + BU_MotionStateInterface& m_motionState; + PolyhedralConvexShape& m_shape; + void* m_userPointer; + +}; + +#endif //BU_COLLIDABLE diff --git a/Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp b/Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp new file mode 100644 index 000000000..ebd80cc31 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_CollisionPair.cpp @@ -0,0 +1,581 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_CollisionPair.h" +#include "NarrowPhaseCollision/BU_VertexPoly.h" +#include "NarrowPhaseCollision/BU_EdgeEdge.h" +#include "BU_Collidable.h" + + +#include "BU_MotionStateInterface.h" +#include "CollisionShapes/PolyhedralConvexShape.h" +#include +#include "SimdTransformUtil.h" + + + +BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance) +: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)), +m_tolerance(tolerance) +{ + +} + +// if there exists a time-of-impact between any feature_pair (edgeA,edgeB), +// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time + + +/* +bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB) + +*/ + +bool BU_CollisionPair::calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result) +{ + + + + + SimdVector3 linvelA,angvelA; + SimdVector3 linvelB,angvelB; + + SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA); + SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB); + + + SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin(); + SimdQuaternion angularMotionA(0,0,0,1.f); + SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin(); + SimdQuaternion angularMotionB(0,0,0,1); + + + + result.m_fraction = 1.f; + + SimdTransform impactTransA; + SimdTransform impactTransB; + + int index=0; + + SimdScalar toiUnscaled=result.m_fraction; + const SimdScalar toiUnscaledLimit = result.m_fraction; + + SimdTransform a2w; + a2w = fromA; + SimdTransform b2w = fromB; + +/* debugging code + { + const int numvertsB = m_convexB->GetNumVertices(); + for (int v=0;vGetVertex(v,pt); + pt = b2w * pt; + char buf[1000]; + + if (pt.y() < 0.) + { + sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } else + { + sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + + } + } + } +*/ + + + SimdTransform b2wp = b2w; + + b2wp.setOrigin(b2w.getOrigin() + linearMotionB); + b2wp.setRotation( b2w.getRotation() + angularMotionB); + + impactTransB = b2wp; + + SimdTransform a2wp; + a2wp.setOrigin(a2w.getOrigin()+ linearMotionA); + a2wp.setRotation(a2w.getRotation()+angularMotionA); + + impactTransA = a2wp; + + SimdTransform a2winv; + a2winv = a2w.inverse(); + + SimdTransform b2wpinv; + b2wpinv = b2wp.inverse(); + + SimdTransform b2winv; + b2winv = b2w.inverse(); + + SimdTransform a2wpinv; + a2wpinv = a2wp.inverse(); + + //Redon's version with concatenated transforms + + SimdTransform relative; + + relative = b2w * b2wpinv * a2wp * a2winv; + + //relative = a2winv * a2wp * b2wpinv * b2w; + + SimdQuaternion qrel; + relative.getBasis().getRotation(qrel); + + SimdVector3 linvel = relative.getOrigin(); + + if (linvel.length() < SCREWEPSILON) + { + linvel.setValue(0.,0.,0.); + } + SimdVector3 angvel; + angvel[0] = 2.f * SimdAsin (qrel[0]); + angvel[1] = 2.f * SimdAsin (qrel[1]); + angvel[2] = 2.f * SimdAsin (qrel[2]); + + if (angvel.length() < SCREWEPSILON) + { + angvel.setValue(0.f,0.f,0.f); + } + + //Redon's version with concatenated transforms + m_screwing = BU_Screwing(linvel,angvel); + + SimdTransform w2s; + m_screwing.LocalMatrix(w2s); + + SimdTransform s2w; + s2w = w2s.inverse(); + + //impactTransA = a2w; + //impactTransB = b2w; + + bool hit = false; + + if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW())) + { + //W = 0 , S = 0 , no collision + //toi = 0; + /* + { + const int numvertsB = m_convexB->GetNumVertices(); + for (int v=0;vGetVertex(v,pt); + pt = impactTransB * pt; + char buf[1000]; + + if (pt.y() < 0.) + { + sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } + else + { + sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } + } + } + */ + + return false;//don't continue moving within epsilon + } + +#define EDGEEDGE +#ifdef EDGEEDGE + + BU_EdgeEdge edgeEdge; + + //for all edged in A check agains all edges in B + for (int ea = 0;ea < m_convexA->GetNumEdges();ea++) + { + SimdPoint3 pA0,pA1; + + m_convexA->GetEdge(ea,pA0,pA1); + + pA0= a2w * pA0;//in world space + pA0 = w2s * pA0;//in screwing space + + pA1= a2w * pA1;//in world space + pA1 = w2s * pA1;//in screwing space + + int numedgesB = m_convexB->GetNumEdges(); + for (int eb = 0; eb < numedgesB;eb++) + { + { + SimdPoint3 pB0,pB1; + m_convexB->GetEdge(eb,pB0,pB1); + + pB0= b2w * pB0;//in world space + pB0 = w2s * pB0;//in screwing space + + pB1= b2w * pB1;//in world space + pB1 = w2s * pB1;//in screwing space + + + SimdScalar lambda,mu; + + toiUnscaled = 1.; + + SimdVector3 edgeDirA(pA1-pA0); + SimdVector3 edgeDirB(pB1-pB0); + + if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu)) + { + //printf("edgeedge potential hit\n"); + if (toiUnscaled>=0) + { + if (toiUnscaled < toiUnscaledLimit) + { + + //inside check is already done by checking the mu and gamma ! + + SimdPoint3 vtx = pA0+lambda * (pA1-pA0); + SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); + + SimdPoint3 hitptWorld = s2w * hitpt; + { + + if (toiUnscaled < result.m_fraction) + result.m_fraction = toiUnscaled; + + hit = true; + + SimdVector3 hitNormal = edgeDirB.cross(edgeDirA); + + hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled); + + + hitNormal.normalize(); + + //an approximated normal can be calculated by taking the cross product of both edges + //take care of the sign ! + + SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; + + SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld); + if (dist > 0) + hitNormalWorld *= -1; + + //todo: this is the wrong point, because b2winv is still at begin of motion + // not at time-of-impact location! + //bhitpt = b2winv * hitptWorld; + +// m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld); + } + + } + } + } + } + + index++; + } + }; +#endif //EDGEEDGE + +#define VERTEXFACE +#ifdef VERTEXFACE + + // for all vertices in A, for each face in B,do vertex-face + { + const int numvertsA = m_convexA->GetNumVertices(); + for (int v=0;vGetVertex(v,vtx); + + vtx = a2w * vtx;//in world space + vtx = w2s * vtx;//in screwing space + + const int numplanesB = m_convexB->GetNumPlanes(); + + for (int p = 0 ; p < numplanesB; p++) + //int p=2; + { + + { + + SimdVector3 planeNorm; + SimdPoint3 planeSupport; + + m_convexB->GetPlane(planeNorm,planeSupport,p); + + + planeSupport = b2w * planeSupport;//transform to world space + SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm; + + planeSupport = w2s * planeSupport ; //transform to screwing space + planeNorm = w2s.getBasis() * planeNormWorld; + + planeNorm.normalize(); + + SimdScalar d = planeSupport.dot(planeNorm); + + SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); + + BU_VertexPoly vtxApolyB; + + toiUnscaled = 1.; + + if ((p==2) && (v==6)) + { +// printf("%f toiUnscaled\n",toiUnscaled); + + } + if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false)) + { + + + + + if (toiUnscaled >= 0. ) + { + //not only collect the first point, get every contactpoint, later we have to check the + //manifold properly! + + if (toiUnscaled <= toiUnscaledLimit) + { + // printf("toiUnscaled %f\n",toiUnscaled ); + + SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); + SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled); + + SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; + SimdPoint3 hitptWorld = s2w * hitpt; + + + hitpt = b2winv * hitptWorld; + //vertex has to be 'within' the facet's boundary + if (m_convexB->IsInside(hitpt,m_tolerance)) + { +// m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld); + + if (toiUnscaled < result.m_fraction) + result.m_fraction= toiUnscaled; + hit = true; + + } + } + } + } + + } + + index++; + } + } + } + + // + // for all vertices in B, for each face in A,do vertex-face + //copy and pasted from all verts A -> all planes B so potential typos! + //todo: make this into one method with a kind of 'swapped' logic + // + { + const int numvertsB = m_convexB->GetNumVertices(); + for (int v=0;vGetVertex(v,vtx); + + vtx = b2w * vtx;//in world space +/* + + char buf[1000]; + + if (vtx.y() < 0.) + { + sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } + sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + +*/ + vtx = w2s * vtx;//in screwing space + + const int numplanesA = m_convexA->GetNumPlanes(); + + for (int p = 0 ; p < numplanesA; p++) + //int p=2; + { + + { + SimdVector3 planeNorm; + SimdPoint3 planeSupport; + + m_convexA->GetPlane(planeNorm,planeSupport,p); + + + planeSupport = a2w * planeSupport;//transform to world space + SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm; + + planeSupport = w2s * planeSupport ; //transform to screwing space + planeNorm = w2s.getBasis() * planeNormWorld; + + planeNorm.normalize(); + + SimdScalar d = planeSupport.dot(planeNorm); + + SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); + + BU_VertexPoly vtxBpolyA; + + toiUnscaled = 1.; + + if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true)) + { + if (toiUnscaled>=0.) + { + if (toiUnscaled < toiUnscaledLimit) + { + SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled); + SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled); + //SimdScalar len = hitNormal.length()-1; + + //assert( SimdFuzzyZero(len) ); + + + SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; + SimdPoint3 hitptWorld = s2w * hitpt; + hitpt = a2winv * hitptWorld; + + + //vertex has to be 'within' the facet's boundary + if (m_convexA->IsInside(hitpt,m_tolerance)) + { + +// m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld); + if (toiUnscaled GetNumVertices(); + for (int v=0;vGetVertex(v,pt); + pt = impactTransB * pt; + char buf[1000]; + + if (pt.y() < 0.) + { + sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } + else + { + sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); + if (debugFile) + fwrite(buf,1,strlen(buf),debugFile); + } + } + } +*/ + return hit; +} + + diff --git a/Bullet/NarrowPhaseCollision/BU_CollisionPair.h b/Bullet/NarrowPhaseCollision/BU_CollisionPair.h new file mode 100644 index 000000000..8dcea3647 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_CollisionPair.h @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_COLLISIONPAIR +#define BU_COLLISIONPAIR + +#include +#include + + +#include + +class PolyhedralConvexShape; + + +///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes. +class BU_CollisionPair : public ConvexCast +{ + +public: + BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f); + //toi + + virtual bool calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result); + + + + +private: + const PolyhedralConvexShape* m_convexA; + const PolyhedralConvexShape* m_convexB; + BU_Screwing m_screwing; + SimdScalar m_tolerance; + +}; +#endif //BU_COLLISIONPAIR diff --git a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp b/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp new file mode 100644 index 000000000..75103c0f7 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_EdgeEdge.cpp @@ -0,0 +1,578 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_EdgeEdge.h" +#include "BU_Screwing.h" +#include +#include + +//#include "BU_IntervalArithmeticPolynomialSolver.h" +#include "BU_AlgebraicPolynomialSolver.h" + +#define USE_ALGEBRAIC +#ifdef USE_ALGEBRAIC +#define BU_Polynomial BU_AlgebraicPolynomialSolver +#else +#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver +#endif + +BU_EdgeEdge::BU_EdgeEdge() +{ +} + + +bool BU_EdgeEdge::GetTimeOfImpact( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lambda1, + SimdScalar& mu1 + + ) +{ + bool hit=false; + + SimdScalar lambda; + SimdScalar mu; + + const SimdScalar w=screwAB.GetW(); + const SimdScalar s=screwAB.GetS(); + + if (SimdFuzzyZero(s) && + SimdFuzzyZero(w)) + { + //no motion, no collision + return false; + } + + if (SimdFuzzyZero(w) ) + { + //pure translation W=0, S <> 0 + //no trig, f(t)=t + SimdScalar det = u.y()*v.x()-u.x()*v.y(); + if (!SimdFuzzyZero(det)) + { + lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det; + mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det; + + if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1) + { + // single potential collision is + SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s; + //if this is on the edge, and time t within [0..1] report hit + if (t>=0 && t <= minTime) + { + hit = true; + lambda1 = lambda; + mu1 = mu; + minTime=t; + } + } + + } else + { + //parallel case, not yet + } + } else + { + if (SimdFuzzyZero(s) ) + { + if (SimdFuzzyZero(u.z()) ) + { + if (SimdFuzzyZero(v.z()) ) + { + //u.z()=0,v.z()=0 + if (SimdFuzzyZero(a.z()-c.z())) + { + //printf("NOT YET planar problem, 4 vertex=edge cases\n"); + + } else + { + //printf("parallel but distinct planes, no collision\n"); + return false; + } + + } else + { + SimdScalar mu = (a.z() - c.z())/v.z(); + if (0<=mu && mu <= 1) + { + // printf("NOT YET//u.z()=0,v.z()<>0\n"); + } else + { + return false; + } + + } + } else + { + //u.z()<>0 + + if (SimdFuzzyZero(v.z()) ) + { + //printf("u.z()<>0,v.z()=0\n"); + lambda = (c.z() - a.z())/u.z(); + if (0<=lambda && lambda <= 1) + { + //printf("u.z()<>0,v.z()=0\n"); + SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f); + SimdScalar r2 = rotPt.length2();//px*px + py*py; + + //either y=a*x+b, or x = a*x+b... + //depends on whether value v.x() is zero or not + SimdScalar aa; + SimdScalar bb; + + if (SimdFuzzyZero(v.x())) + { + aa = v.x()/v.y(); + bb= c.x()+ (-c.y() /v.y()) *v.x(); + } else + { + //line is c+mu*v; + //x = c.x()+mu*v.x(); + //mu = ((x-c.x())/v.x()); + //y = c.y()+((x-c.x())/v.x())*v.y(); + //y = c.y()+ (-c.x() /v.x()) *v.y() + (x /v.x()) *v.y(); + //y = a*x+b,where a = v.y()/v.x(), b= c.y()+ (-c.x() /v.x()) *v.y(); + aa = v.y()/v.x(); + bb= c.y()+ (-c.x() /v.x()) *v.y(); + } + + SimdScalar disc = aa*aa*r2 + r2 - bb*bb; + if (disc <0) + { + //edge doesn't intersect the circle (motion of the vertex) + return false; + } + SimdScalar rad = SimdSqrt(r2); + + if (SimdFuzzyZero(disc)) + { + SimdPoint3 intersectPt; + + SimdScalar mu; + //intersectionPoint edge with circle; + if (SimdFuzzyZero(v.x())) + { + intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1))); + intersectPt.setX( aa*intersectPt.y()+bb ); + mu = ((intersectPt.y()-c.y())/v.y()); + } else + { + intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1))); + intersectPt.setY(aa*intersectPt.x()+bb); + mu = ((intersectPt.getX()-c.getX())/v.getX()); + + } + + if (0 <= mu && mu <= 1) + { + hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); + } + //only one solution + } else + { + //two points... + //intersectionPoint edge with circle; + SimdPoint3 intersectPt; + //intersectionPoint edge with circle; + if (SimdFuzzyZero(v.x())) + { + SimdScalar mu; + + intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); + intersectPt.setX(aa*intersectPt.y()+bb); + mu = ((intersectPt.getY()-c.getY())/v.getY()); + if (0.f <= mu && mu <= 1.f) + { + hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); + } + intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); + intersectPt.setX(aa*intersectPt.y()+bb); + mu = ((intersectPt.getY()-c.getY())/v.getY()); + if (0 <= mu && mu <= 1) + { + hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); + } + + } else + { + SimdScalar mu; + + intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f))); + intersectPt.setY(aa*intersectPt.x()+bb); + mu = ((intersectPt.getX()-c.getX())/v.getX()); + if (0 <= mu && mu <= 1) + { + hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); + } + intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f))); + intersectPt.setY(aa*intersectPt.x()+bb); + mu = ((intersectPt.getX()-c.getX())/v.getX()); + if (0.f <= mu && mu <= 1.f) + { + hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime); + } + } + } + + + + //int k=0; + + } else + { + return false; + } + + + } else + { + //u.z()<>0,v.z()<>0 + //printf("general case with s=0\n"); + hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu); + if (hit) + { + lambda1 = lambda; + mu1 = mu; + + } + } + } + + } else + { + //printf("general case, W<>0,S<>0\n"); + hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu); + if (hit) + { + lambda1 = lambda; + mu1 = mu; + } + + } + + + //W <> 0,pure rotation + } + + return hit; +} + + +bool BU_EdgeEdge::GetTimeOfImpactGeneralCase( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lambda, + SimdScalar& mu + + ) +{ + bool hit = false; + + SimdScalar coefs[4]={0.f,0.f,0.f,0.f}; + BU_Polynomial polynomialSolver; + int numroots = 0; + + //SimdScalar eps=1e-15f; + //SimdScalar eps2=1e-20f; + SimdScalar s=screwAB.GetS(); + SimdScalar w = screwAB.GetW(); + + SimdScalar ax = a.x(); + SimdScalar ay = a.y(); + SimdScalar az = a.z(); + SimdScalar cx = c.x(); + SimdScalar cy = c.y(); + SimdScalar cz = c.z(); + SimdScalar vx = v.x(); + SimdScalar vy = v.y(); + SimdScalar vz = v.z(); + SimdScalar ux = u.x(); + SimdScalar uy = u.y(); + SimdScalar uz = u.z(); + + + if (!SimdFuzzyZero(v.z())) + { + + //Maple Autogenerated C code + SimdScalar t1,t2,t3,t4,t7,t8,t10; + SimdScalar t13,t14,t15,t16,t17,t18,t19,t20; + SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30; + SimdScalar t31,t32,t33,t34,t35,t36,t39,t40; + SimdScalar t41,t43,t48; + SimdScalar t63; + + SimdScalar aa,bb,cc,dd;//the coefficients + + t1 = v.y()*s; t2 = t1*u.x(); + t3 = v.x()*s; + t4 = t3*u.y(); + t7 = SimdTan(w/2.0f); + t8 = 1.0f/t7; + t10 = 1.0f/v.z(); + aa = (t2-t4)*t8*t10; + t13 = a.x()*t7; + t14 = u.z()*v.y(); + t15 = t13*t14; + t16 = u.x()*v.z(); + t17 = a.y()*t7; + t18 = t16*t17; + t19 = u.y()*v.z(); + t20 = t13*t19; + t21 = v.y()*u.x(); + t22 = c.z()*t7; + t23 = t21*t22; + t24 = v.x()*a.z(); + t25 = t7*u.y(); + t26 = t24*t25; + t27 = c.y()*t7; + t28 = t16*t27; + t29 = a.z()*t7; + t30 = t21*t29; + t31 = u.z()*v.x(); + t32 = t31*t27; + t33 = t31*t17; + t34 = c.x()*t7; + t35 = t34*t19; + t36 = t34*t14; + t39 = v.x()*c.z(); + t40 = t39*t25; + t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40; + bb = t41*t8*t10; + t43 = t7*u.x(); + t48 = u.y()*v.y(); + cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2 + -2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10; + t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30; + dd = t63*t8*t10; + + coefs[0]=aa; + coefs[1]=bb; + coefs[2]=cc; + coefs[3]=dd; + + } else + { + + SimdScalar t1,t2,t3,t4,t7,t8,t10; + SimdScalar t13,t14,t15,t16,t17,t18,t19,t20; + SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30; + SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57; + SimdScalar p1,p2,p3,p4; + + t1 = uy*s; + t2 = t1*vx; + t3 = ux*s; + t4 = t3*vy; + t7 = SimdTan(w/2.0f); + t8 = 1/t7; + t10 = 1/uz; + t13 = ux*az; + t14 = t7*vy; + t15 = t13*t14; + t16 = ax*t7; + t17 = uy*vz; + t18 = t16*t17; + t19 = cx*t7; + t20 = t19*t17; + t21 = vy*uz; + t22 = t19*t21; + t23 = ay*t7; + t24 = vx*uz; + t25 = t23*t24; + t26 = uy*cz; + t27 = t7*vx; + t28 = t26*t27; + t29 = t16*t21; + t30 = cy*t7; + t31 = ux*vz; + t32 = t30*t31; + t33 = ux*cz; + t34 = t33*t14; + t35 = t23*t31; + t36 = t30*t24; + t37 = uy*az; + t38 = t37*t27; + + p4 = (-t2+t4)*t8*t10; + p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx; + p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13 +*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24; + t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38; + p1 = t57*t8*t10; + + coefs[0] = p4; + coefs[1] = p3; + coefs[2] = p2; + coefs[1] = p1; + + } + + numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]); + + for (int i=0;i=0.f && t= -0.001); + + //if (hit) + { + // minTime = 0; + //calculate the time of impact, using the fact of + //toi = alpha / screwAB.getW(); + // cos (alpha) = adjacent/hypothenuse; + //adjacent = dotproduct(ipedge,point); + //hypothenuse = sqrt(r2); + SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius; + SimdScalar hypo = rotRadius; + SimdScalar alpha = SimdAcos(adjacent/hypo); + SimdScalar t = alpha / rotW; + if (t >= 0 && t < minTime) + { + hit = true; + minTime = t; + } else + { + hit = false; + } + + } + return hit; +} + +bool BU_EdgeEdge::GetTimeOfImpactVertexEdge( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lamda, + SimdScalar& mu + + ) +{ + return false; +} diff --git a/Bullet/NarrowPhaseCollision/BU_EdgeEdge.h b/Bullet/NarrowPhaseCollision/BU_EdgeEdge.h new file mode 100644 index 000000000..4823f1dee --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_EdgeEdge.h @@ -0,0 +1,76 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_EDGEEDGE +#define BU_EDGEEDGE + +class BU_Screwing; +#include +#include +#include + +//class BUM_Point2; + +#include + +///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges. +class BU_EdgeEdge +{ +public: + + + BU_EdgeEdge(); + bool GetTimeOfImpact( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lamda, + SimdScalar& mu + ); +private: + + bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime); + bool GetTimeOfImpactGeneralCase( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lamda, + SimdScalar& mu + + ); + + + bool GetTimeOfImpactVertexEdge( + const BU_Screwing& screwAB, + const SimdPoint3& a,//edge in object A + const SimdVector3& u, + const SimdPoint3& c,//edge in object B + const SimdVector3& v, + SimdScalar &minTime, + SimdScalar &lamda, + SimdScalar& mu + + ); + +}; + +#endif //BU_EDGEEDGE diff --git a/Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h b/Bullet/NarrowPhaseCollision/BU_MotionStateInterface.h new file mode 100644 index 000000000..d06a8fab0 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_MotionStateInterface.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 BU_MOTIONSTATE +#define BU_MOTIONSTATE + + +#include +#include +#include + +class BU_MotionStateInterface +{ +public: + virtual ~BU_MotionStateInterface(){}; + + virtual void SetTransform(const SimdTransform& trans) = 0; + virtual void GetTransform(SimdTransform& trans) const = 0; + + virtual void SetPosition(const SimdPoint3& position) = 0; + virtual void GetPosition(SimdPoint3& position) const = 0; + + virtual void SetOrientation(const SimdQuaternion& orientation) = 0; + virtual void GetOrientation(SimdQuaternion& orientation) const = 0; + + virtual void SetBasis(const SimdMatrix3x3& basis) = 0; + virtual void GetBasis(SimdMatrix3x3& basis) const = 0; + + virtual void SetLinearVelocity(const SimdVector3& linvel) = 0; + virtual void GetLinearVelocity(SimdVector3& linvel) const = 0; + + virtual void GetAngularVelocity(SimdVector3& angvel) const = 0; + virtual void SetAngularVelocity(const SimdVector3& angvel) = 0; + +}; + +#endif //BU_MOTIONSTATE diff --git a/Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h b/Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h new file mode 100644 index 000000000..15d67faee --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_PolynomialSolverInterface.h @@ -0,0 +1,39 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 BUM_POLYNOMIAL_SOLVER_INTERFACE +#define BUM_POLYNOMIAL_SOLVER_INTERFACE + +#include +// +//BUM_PolynomialSolverInterface is interface class for polynomial root finding. +//The number of roots is returned as a result, query GetRoot to get the actual solution. +// +class BUM_PolynomialSolverInterface +{ +public: + virtual ~BUM_PolynomialSolverInterface() {}; + + +// virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0; + + virtual int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0; + + virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0; + + virtual SimdScalar GetRoot(int i) const = 0; + +}; + +#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE diff --git a/Bullet/NarrowPhaseCollision/BU_Screwing.cpp b/Bullet/NarrowPhaseCollision/BU_Screwing.cpp new file mode 100644 index 000000000..997fcc3a9 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_Screwing.cpp @@ -0,0 +1,200 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Stephane Redon / Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_Screwing.h" + + +BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) { + + + const SimdScalar dx=relLinVel[0]; + const SimdScalar dy=relLinVel[1]; + const SimdScalar dz=relLinVel[2]; + const SimdScalar wx=relAngVel[0]; + const SimdScalar wy=relAngVel[1]; + const SimdScalar wz=relAngVel[2]; + + // Compute the screwing parameters : + // w : total amount of rotation + // s : total amount of translation + // u : vector along the screwing axis (||u||=1) + // o : point on the screwing axis + + m_w=SimdSqrt(wx*wx+wy*wy+wz*wz); + //if (!w) { + if (fabs(m_w)= BUM_EPSILON2) { + if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector + n1.normalize(); + SimdVector3 n1orth=m_u.cross(n1); + + float n2x=SimdCos(0.5f*m_w); + float n2y=SimdSin(0.5f*m_w); + + m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth); + } + else + { + m_o=SimdPoint3(0.f,0.f,0.f); + } + + } + +} + +//Then, I need to compute Pa, the matrix from the reference (global) frame to +//the screwing frame : + + +void BU_Screwing::LocalMatrix(SimdTransform &t) const { +//So the whole computations do this : align the Oz axis along the +// screwing axis (thanks to u), and then find two others orthogonal axes to +// complete the basis. + + if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON)) + { + // to avoid numerical problems + float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]); + float invn=1.0f/n; + SimdMatrix3x3 mat; + + mat[0][0]=-m_u[1]*invn; + mat[0][1]=m_u[0]*invn; + mat[0][2]=0.f; + + mat[1][0]=-m_u[0]*invn*m_u[2]; + mat[1][1]=-m_u[1]*invn*m_u[2]; + mat[1][2]=n; + + mat[2][0]=m_u[0]; + mat[2][1]=m_u[1]; + mat[2][2]=m_u[2]; + + t.setOrigin(SimdPoint3( + m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn, + -(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n), + -(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2]))); + + t.setBasis(mat); + + } + else { + + SimdMatrix3x3 m; + + m[0][0]=1.; + m[1][0]=0.; + m[2][0]=0.; + + m[0][1]=0.f; + m[1][1]=float(SimdSign(m_u[2])); + m[2][1]=0.f; + + m[0][2]=0.f; + m[1][2]=0.f; + m[2][2]=float(SimdSign(m_u[2])); + + t.setOrigin(SimdPoint3( + -m_o[0], + -SimdSign(m_u[2])*m_o[1], + -SimdSign(m_u[2])*m_o[2] + )); + t.setBasis(m); + + } +} + +//gives interpolated transform for time in [0..1] in screwing frame +SimdTransform BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const +{ + SimdPoint3 org = tr.getOrigin(); + + SimdPoint3 neworg ( + org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t), + org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t), + org.z()+m_s*CalculateF(t)); + + SimdTransform newtr; + newtr.setOrigin(neworg); + SimdMatrix3x3 basis = tr.getBasis(); + SimdMatrix3x3 basisorg = tr.getBasis(); + + SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t); + SimdQuaternion tmpOrn; + tr.getBasis().getRotation(tmpOrn); + rot = rot * tmpOrn; + + //to avoid numerical drift, normalize quaternion + rot.normalize(); + newtr.setBasis(SimdMatrix3x3(rot)); + return newtr; + +} + + +SimdScalar BU_Screwing::CalculateF(SimdScalar t) const +{ + SimdScalar result; + if (!m_w) + { + result = t; + } else + { + result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f)); + } + return result; +} + diff --git a/Bullet/NarrowPhaseCollision/BU_Screwing.h b/Bullet/NarrowPhaseCollision/BU_Screwing.h new file mode 100644 index 000000000..17603cec0 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_Screwing.h @@ -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. +*/ + + +#ifndef B_SCREWING_H +#define B_SCREWING_H + + +#include +#include +#include + + +#define SCREWEPSILON 0.00001f + +///BU_Screwing implements screwing motion interpolation. +class BU_Screwing +{ +public: + + + BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel); + + ~BU_Screwing() { + }; + + SimdScalar CalculateF(SimdScalar t) const; + //gives interpolated position for time in [0..1] in screwing frame + + inline SimdPoint3 InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const + { + return SimdPoint3( + pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t), + pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t), + pt.z()+m_s*CalculateF(t)); + } + + inline SimdVector3 InBetweenVector(const SimdVector3& vec,SimdScalar t) const + { + return SimdVector3( + vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t), + vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t), + vec.z()); + } + + //gives interpolated transform for time in [0..1] in screwing frame + SimdTransform InBetweenTransform(const SimdTransform& tr,SimdScalar t) const; + + + //gives matrix from global frame into screwing frame + void LocalMatrix(SimdTransform &t) const; + + inline const SimdVector3& GetU() const { return m_u;} + inline const SimdVector3& GetO() const {return m_o;} + inline const SimdScalar GetS() const{ return m_s;} + inline const SimdScalar GetW() const { return m_w;} + +private: + float m_w; + float m_s; + SimdVector3 m_u; + SimdVector3 m_o; +}; + +#endif //B_SCREWING_H diff --git a/Bullet/NarrowPhaseCollision/BU_StaticMotionState.h b/Bullet/NarrowPhaseCollision/BU_StaticMotionState.h new file mode 100644 index 000000000..4202b3dde --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_StaticMotionState.h @@ -0,0 +1,91 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_STATIC_MOTIONSTATE +#define BU_STATIC_MOTIONSTATE + + +#include + +class BU_StaticMotionState :public BU_MotionStateInterface +{ +public: + virtual ~BU_StaticMotionState(){}; + + virtual void SetTransform(const SimdTransform& trans) + { + m_trans = trans; + } + virtual void GetTransform(SimdTransform& trans) const + { + trans = m_trans; + } + virtual void SetPosition(const SimdPoint3& position) + { + m_trans.setOrigin( position ); + } + virtual void GetPosition(SimdPoint3& position) const + { + position = m_trans.getOrigin(); + } + + virtual void SetOrientation(const SimdQuaternion& orientation) + { + m_trans.setRotation( orientation); + } + virtual void GetOrientation(SimdQuaternion& orientation) const + { + orientation = m_trans.getRotation(); + } + + virtual void SetBasis(const SimdMatrix3x3& basis) + { + m_trans.setBasis( basis); + } + virtual void GetBasis(SimdMatrix3x3& basis) const + { + basis = m_trans.getBasis(); + } + + virtual void SetLinearVelocity(const SimdVector3& linvel) + { + m_linearVelocity = linvel; + } + virtual void GetLinearVelocity(SimdVector3& linvel) const + { + linvel = m_linearVelocity; + } + + virtual void SetAngularVelocity(const SimdVector3& angvel) + { + m_angularVelocity = angvel; + } + virtual void GetAngularVelocity(SimdVector3& angvel) const + { + angvel = m_angularVelocity; + } + + + +protected: + + SimdTransform m_trans; + SimdVector3 m_angularVelocity; + SimdVector3 m_linearVelocity; + +}; + +#endif //BU_STATIC_MOTIONSTATE diff --git a/Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp b/Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp new file mode 100644 index 000000000..71de3a284 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/BU_VertexPoly.cpp @@ -0,0 +1,159 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_VertexPoly.h" +#include "BU_Screwing.h" +#include +#include +#include + +#define USE_ALGEBRAIC +#ifdef USE_ALGEBRAIC + #include "BU_AlgebraicPolynomialSolver.h" + #define BU_Polynomial BU_AlgebraicPolynomialSolver +#else + #include "BU_IntervalArithmeticPolynomialSolver.h" + #define BU_Polynomial BU_IntervalArithmeticPolynomialSolver +#endif + +inline bool TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; } + + +BU_VertexPoly::BU_VertexPoly() +{ + +} +//return true if a collision will occur between [0..1] +//false otherwise. If true, minTime contains the time of impact +bool BU_VertexPoly::GetTimeOfImpact( + const BU_Screwing& screwAB, + const SimdPoint3& a, + const SimdVector4& planeEq, + SimdScalar &minTime,bool swapAB) +{ + + bool hit = false; + + // precondition: s=0 and w= 0 is catched by caller! + if (TestFuzzyZero(screwAB.GetS()) && + TestFuzzyZero(screwAB.GetW())) + { + return false; + } + + + //case w<>0 and s<> 0 + const SimdScalar w=screwAB.GetW(); + const SimdScalar s=screwAB.GetS(); + + SimdScalar coefs[4]; + const SimdScalar p=planeEq[0]; + const SimdScalar q=planeEq[1]; + const SimdScalar r=planeEq[2]; + const SimdScalar d=planeEq[3]; + + const SimdVector3 norm(p,q,r); + BU_Polynomial polynomialSolver; + int numroots = 0; + + //SimdScalar eps=1e-80f; + //SimdScalar eps2=1e-100f; + + if (TestFuzzyZero(screwAB.GetS()) ) + { + //S = 0 , W <> 0 + + //ax^3+bx^2+cx+d=0 + coefs[0]=0.; + coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d); + coefs[2]=-2*p*a.y()+2*q*a.x(); + coefs[3]=p*a.x()+q*a.y()+r*a.z()-d; + +// numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]); + numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]); + + } else + { + if (TestFuzzyZero(screwAB.GetW())) + { + // W = 0 , S <> 0 + //pax+qay+r(az+st)=d + + SimdScalar dist = (d - a.dot(norm)); + + if (TestFuzzyZero(r)) + { + if (TestFuzzyZero(dist)) + { + // no hit + } else + { + // todo a a' might hit sides of polygon T + //printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n"); + } + + } else + { + SimdScalar etoi = (dist)/(r*screwAB.GetS()); + if (swapAB) + etoi *= -1; + + if (etoi >= 0. && etoi <= minTime) + { + minTime = etoi; + hit = true; + } + } + + } else + { + //ax^3+bx^2+cx+d=0 + + //degenerate coefficients mess things up :( + SimdScalar ietsje = (r*s)/SimdTan(w/2.f); + if (ietsje*ietsje < 0.01f) + ietsje = 0.f; + + coefs[0]=ietsje;//(r*s)/tan(w/2.); + coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d); + coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.))); + coefs[3]=p*a.x()+q*a.y()+r*a.z()-d; + + numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]); + } + } + + + for (int i=0;i=0 && t +#include +#include + +///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane. +class BU_VertexPoly +{ +public: + BU_VertexPoly(); + bool GetTimeOfImpact( + const BU_Screwing& screwAB, + const SimdPoint3& vtx, + const SimdVector4& planeEq, + SimdScalar &minTime, + bool swapAB); + +private: + + //cached data (frame coherency etc.) here + +}; +#endif //VERTEX_POLY_H diff --git a/Bullet/NarrowPhaseCollision/CollisionMargin.h b/Bullet/NarrowPhaseCollision/CollisionMargin.h new file mode 100644 index 000000000..377f0e506 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/CollisionMargin.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/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp b/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.cpp new file mode 100644 index 000000000..710208a91 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.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 "ContinuousConvexCollision.h" +#include "CollisionShapes/ConvexShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" +#include "NarrowPhaseCollision/SimplexSolverInterface.h" +#include "SimdTransformUtil.h" +#include "CollisionShapes/SphereShape.h" + +#include "GjkPairDetector.h" +#include "PointCollector.h" + + + +ContinuousConvexCollision::ContinuousConvexCollision ( ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver, ConvexPenetrationDepthSolver* 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 ContinuousConvexCollision::calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result) +{ + + m_simplexSolver->reset(); + + /// compute linear and angular velocity for this interval, to interpolate + SimdVector3 linVelA,angVelA,linVelB,angVelB; + SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linVelA,angVelA); + SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linVelB,angVelB); + + SimdScalar boundingRadiusA = m_convexA->GetAngularMotionDisc(); + SimdScalar boundingRadiusB = m_convexB->GetAngularMotionDisc(); + + SimdScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; + + float radius = 0.001f; + + SimdScalar lambda = 0.f; + SimdVector3 v(1,0,0); + + int maxIter = MAX_ITERATIONS; + + SimdVector3 n; + n.setValue(0.f,0.f,0.f); + bool hasResult = false; + SimdVector3 c; + + float lastLambda = lambda; + //float epsilon = 0.001f; + + int numIter = 0; + //first solution, using GJK + + + SimdTransform identityTrans; + identityTrans.setIdentity(); + + SphereShape raySphere(0.0f); + raySphere.SetMargin(0.f); + + +// result.DrawCoordSystem(sphereTr); + + PointCollector pointCollector1; + + { + + GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + GjkPairDetector::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) + { + SimdScalar 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 + SimdTransform interpolatedTransA,interpolatedTransB,relativeTrans; + + SimdTransformUtil::IntegrateTransform(fromA,linVelA,angVelA,lambda,interpolatedTransA); + SimdTransformUtil::IntegrateTransform(fromB,linVelB,angVelB,lambda,interpolatedTransB); + relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA); + + result.DebugDraw( lambda ); + + PointCollector pointCollector; + GjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); + GjkPairDetector::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 + SimdVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); + if (result.m_fraction < 1.f) + { + if (move.dot(result.m_normal) <= 0.f) + { + } + } +*/ + +} + diff --git a/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.h b/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.h new file mode 100644 index 000000000..0128f19f6 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ContinuousConvexCollision.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 "ConvexCast.h" +#include "SimplexSolverInterface.h" +class ConvexPenetrationDepthSolver; +class ConvexShape; + +/// ContinuousConvexCollision 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 ContinuousConvexCollision : public ConvexCast +{ + SimplexSolverInterface* m_simplexSolver; + ConvexPenetrationDepthSolver* m_penetrationDepthSolver; + ConvexShape* m_convexA; + ConvexShape* m_convexB; + + +public: + + ContinuousConvexCollision (ConvexShape* shapeA,ConvexShape* shapeB ,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver); + + virtual bool calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result); + + +}; + +#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H + diff --git a/Bullet/NarrowPhaseCollision/ConvexCast.cpp b/Bullet/NarrowPhaseCollision/ConvexCast.cpp new file mode 100644 index 000000000..689655086 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ConvexCast.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 "ConvexCast.h" + +ConvexCast::~ConvexCast() +{ +} diff --git a/Bullet/NarrowPhaseCollision/ConvexCast.h b/Bullet/NarrowPhaseCollision/ConvexCast.h new file mode 100644 index 000000000..22c0966df --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ConvexCast.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 +#include +#include +class MinkowskiSumShape; +#include "IDebugDraw.h" + +/// ConvexCast is an interface for Casting +class ConvexCast +{ +public: + + + virtual ~ConvexCast(); + + ///RayResult stores the closest result + /// alternatively, add a callback method to decide about closest/all results + struct CastResult + { + //virtual bool addRayResult(const SimdVector3& normal,SimdScalar fraction) = 0; + + virtual void DebugDraw(SimdScalar fraction) {} + virtual void DrawCoordSystem(const SimdTransform& trans) {} + + CastResult() + :m_fraction(1e30f), + m_debugDrawer(0) + { + } + + + virtual ~CastResult() {}; + + SimdVector3 m_normal; + SimdScalar m_fraction; + SimdTransform m_hitTransformA; + SimdTransform m_hitTransformB; + + IDebugDraw* m_debugDrawer; + + }; + + + /// cast a convex against another convex object + virtual bool calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result) = 0; +}; + +#endif //CONVEX_CAST_H diff --git a/Bullet/NarrowPhaseCollision/ConvexPenetrationDepthSolver.h b/Bullet/NarrowPhaseCollision/ConvexPenetrationDepthSolver.h new file mode 100644 index 000000000..8f8c0a97e --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ConvexPenetrationDepthSolver.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 SimdVector3; +#include "SimplexSolverInterface.h" +class ConvexShape; +#include "SimdPoint3.h" +class SimdTransform; + +///ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. +class ConvexPenetrationDepthSolver +{ +public: + + virtual ~ConvexPenetrationDepthSolver() {}; + virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* convexA,ConvexShape* convexB, + const SimdTransform& transA,const SimdTransform& transB, + SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb, + class IDebugDraw* debugDraw + ) = 0; + + +}; +#endif //CONVEX_PENETRATION_DEPTH_H + diff --git a/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h b/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h new file mode 100644 index 000000000..3d6fdb2be --- /dev/null +++ b/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h @@ -0,0 +1,86 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "SimdTransform.h" +#include "SimdVector3.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 DiscreteCollisionDetectorInterface +{ + void operator delete(void* ptr) {}; + + struct Result + { + void operator delete(void* ptr) {}; + + virtual ~Result(){} + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0; + }; + + struct ClosestPointInput + { + ClosestPointInput() + :m_maximumDistanceSquared(1e30f) + { + } + + SimdTransform m_transformA; + SimdTransform m_transformB; + SimdScalar m_maximumDistanceSquared; + }; + + virtual ~DiscreteCollisionDetectorInterface() {}; + + // + // 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 IDebugDraw* debugDraw) = 0; + + SimdScalar getCollisionMargin() { return 0.2f;} +}; + +struct StorageResult : public DiscreteCollisionDetectorInterface::Result +{ + SimdVector3 m_normalOnSurfaceB; + SimdVector3 m_closestPointInB; + SimdScalar m_distance; //negative means penetration ! + + StorageResult() : m_distance(1e30f) + { + + } + virtual ~StorageResult() {}; + + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& 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/Bullet/NarrowPhaseCollision/Epa.cpp b/Bullet/NarrowPhaseCollision/Epa.cpp new file mode 100644 index 000000000..190e4e8ce --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Epa.cpp @@ -0,0 +1,560 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "SimdScalar.h" +#include "SimdVector3.h" +#include "SimdPoint3.h" +#include "SimdTransform.h" +#include "SimdMinMax.h" + +#include "CollisionShapes/ConvexShape.h" + +#include +#include +#include + +#include "NarrowPhaseCollision/SimplexSolverInterface.h" + +#include "NarrowPhaseCollision/EpaCommon.h" + +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" +#include "NarrowPhaseCollision/EpaPolyhedron.h" +#include "NarrowPhaseCollision/Epa.h" + +const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f; +const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR; + +Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB, + const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ), + m_pConvexShapeB( pConvexShapeB ), + m_transformA( transformA ), + m_transformB( transformB ) +{ + m_faceEntries.reserve( EPA_MAX_FACE_ENTRIES ); +} + +Epa::~Epa() +{ +} + +bool Epa::Initialize( SimplexSolverInterface& simplexSolver ) +{ + // Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO + + SimdVector3 v( 1, 0, 0 ); + SimdScalar squaredDistance = SIMD_INFINITY; + + SimdScalar delta = 0.f; + + simplexSolver.reset(); + + int nbIterations = 0; + + while ( true ) + { + assert( ( v.length2() > 0 ) && "Warning : v has zero magnitude!" ); + + SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis(); + SimdVector3 seperatingAxisInB = v * m_transformB.getBasis(); + + SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + SimdPoint3 pWorld = m_transformA( pInA ); + SimdPoint3 qWorld = m_transformB( qInB ); + + SimdVector3 w = pWorld - qWorld; + delta = v.dot( w ); + + assert( ( delta <= 0 ) && "Shapes are disjoint, EPA should have never been called!" ); + assert( !simplexSolver.inSimplex( w ) && "Shapes are disjoint, EPA should have never been called!" ); + + // Add support point to simplex + simplexSolver.addVertex( w, pWorld, qWorld ); + + bool closestOk = simplexSolver.closest( v ); + assert( closestOk && "Shapes are disjoint, EPA should have never been called!" ); + + SimdScalar prevVSqrd = squaredDistance; + squaredDistance = v.length2(); + + // Is v converging to v(A-B) ? + assert( ( ( prevVSqrd - squaredDistance ) > SIMD_EPSILON * prevVSqrd ) && + "Shapes are disjoint, EPA should have never been called!" ); + + if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) + { + break; + } + + ++nbIterations; + } + + SimdPoint3 simplexPoints[ 5 ]; + SimdPoint3 wSupportPointsOnA[ 5 ]; + SimdPoint3 wSupportPointsOnB[ 5 ]; + + int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints ); + + // nbSimplexPoints can't be one because cases where the origin is on the boundary are handled + // by hybrid penetration depth + assert( ( ( nbSimplexPoints > 1 ) && ( nbSimplexPoints <= 4 ) ) && + "Hybrid Penetration Depth algorithm failed!" ); + + int nbPolyhedronPoints = nbSimplexPoints; + +#ifndef EPA_POLYHEDRON_USE_PLANES + int initTetraIndices[ 4 ] = { 0, 1, 2, 3 }; +#endif + + // Prepare initial polyhedron to start EPA from + if ( nbSimplexPoints == 1 ) + { + return false; + } + else if ( nbSimplexPoints == 2 ) + { + // We have a line segment inside the CSO that contains the origin + // Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points + + SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ]; + d.normalize(); + + SimdVector3 v1; + SimdVector3 v2; + SimdVector3 v3; + + SimdVector3 e1; + + SimdScalar absx = abs( d.getX() ); + SimdScalar absy = abs( d.getY() ); + SimdScalar absz = abs( d.getZ() ); + + if ( absx < absy ) + { + if ( absx < absz ) + { + e1.setX( 1 ); + } + else + { + e1.setZ( 1 ); + } + } + else + { + if ( absy < absz ) + { + e1.setY( 1 ); + } + else + { + e1.setZ( 1 ); + } + } + + v1 = d.cross( e1 ); + v1.normalize(); + + v2 = v1.rotate( d, 120 * SIMD_RADS_PER_DEG ); + v3 = v2.rotate( d, 120 * SIMD_RADS_PER_DEG ); + + nbPolyhedronPoints = 5; + + SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis(); + SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis(); + + SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + SimdPoint3 pWorld = m_transformA( p ); + SimdPoint3 qWorld = m_transformB( q ); + + wSupportPointsOnA[ 2 ] = pWorld; + wSupportPointsOnB[ 2 ] = qWorld; + simplexPoints[ 2 ] = wSupportPointsOnA[ 2 ] - wSupportPointsOnB[ 2 ]; + + seperatingAxisInA = v2 * m_transformA.getBasis(); + seperatingAxisInB = -v2 * m_transformB.getBasis(); + + p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + pWorld = m_transformA( p ); + qWorld = m_transformB( q ); + + wSupportPointsOnA[ 3 ] = pWorld; + wSupportPointsOnB[ 3 ] = qWorld; + simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ]; + + seperatingAxisInA = v3 * m_transformA.getBasis(); + seperatingAxisInB = -v3 * m_transformB.getBasis(); + + p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + pWorld = m_transformA( p ); + qWorld = m_transformB( q ); + + wSupportPointsOnA[ 4 ] = pWorld; + wSupportPointsOnB[ 4 ] = qWorld; + simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ]; + +#ifndef EPA_POLYHEDRON_USE_PLANES + if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) ) + { + initTetraIndices[ 1 ] = 2; + initTetraIndices[ 2 ] = 3; + initTetraIndices[ 3 ] = 4; + } + else + { + if ( TetrahedronContainsOrigin( simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ], simplexPoints[ 4 ] ) ) + { + initTetraIndices[ 0 ] = 1; + initTetraIndices[ 1 ] = 2; + initTetraIndices[ 2 ] = 3; + initTetraIndices[ 3 ] = 4; + } + else + { + // No tetrahedron contains the origin + assert( false && "Unable to find an initial tetrahedron that contains the origin!" ); + return false; + } + } +#endif + } + else if ( nbSimplexPoints == 3 ) + { + // We have a triangle inside the CSO that contains the origin + // Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points + + SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ]; + SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ]; + SimdVector3 triangleNormal = v0.cross( v1 ); + triangleNormal.normalize(); + + nbPolyhedronPoints = 5; + + SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis(); + SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis(); + + SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + SimdPoint3 pWorld = m_transformA( p ); + SimdPoint3 qWorld = m_transformB( q ); + + wSupportPointsOnA[ 3 ] = pWorld; + wSupportPointsOnB[ 3 ] = qWorld; + simplexPoints[ 3 ] = wSupportPointsOnA[ 3 ] - wSupportPointsOnB[ 3 ]; + +#ifndef EPA_POLYHEDRON_USE_PLANES + // We place this check here because if the tetrahedron contains the origin + // there is no need to sample another support point + if ( !TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 3 ] ) ) + { +#endif + seperatingAxisInA = -triangleNormal * m_transformA.getBasis(); + seperatingAxisInB = triangleNormal * m_transformB.getBasis(); + + p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + pWorld = m_transformA( p ); + qWorld = m_transformB( q ); + + wSupportPointsOnA[ 4 ] = pWorld; + wSupportPointsOnB[ 4 ] = qWorld; + simplexPoints[ 4 ] = wSupportPointsOnA[ 4 ] - wSupportPointsOnB[ 4 ]; + +#ifndef EPA_POLYHEDRON_USE_PLANES + if ( TetrahedronContainsOrigin( simplexPoints[ 0 ], simplexPoints[ 1 ], simplexPoints[ 2 ], simplexPoints[ 4 ] ) ) + { + initTetraIndices[ 3 ] = 4; + } + else + { + // No tetrahedron contains the origin + assert( false && "Unable to find an initial tetrahedron that contains the origin!" ); + return false; + } + } +#endif + } +#ifdef _DEBUG + else if ( nbSimplexPoints == 4 ) + { + assert( TetrahedronContainsOrigin( simplexPoints ) && "Initial tetrahedron does not contain the origin!" ); + } +#endif + +#ifndef EPA_POLYHEDRON_USE_PLANES + SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ], + simplexPoints[ initTetraIndices[ 1 ] ], + simplexPoints[ initTetraIndices[ 2 ] ], + simplexPoints[ initTetraIndices[ 3 ] ] }; + + SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ], + wSupportPointsOnA[ initTetraIndices[ 1 ] ], + wSupportPointsOnA[ initTetraIndices[ 2 ] ], + wSupportPointsOnA[ initTetraIndices[ 3 ] ] }; + + SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ], + wSupportPointsOnB[ initTetraIndices[ 1 ] ], + wSupportPointsOnB[ initTetraIndices[ 2 ] ], + wSupportPointsOnB[ initTetraIndices[ 3 ] ] }; +#endif + +#ifdef EPA_POLYHEDRON_USE_PLANES + if ( !m_polyhedron.Create( simplexPoints, wSupportPointsOnA, wSupportPointsOnB, nbPolyhedronPoints ) ) +#else + if ( !m_polyhedron.Create( wTetraPoints, wTetraSupportPointsOnA, wTetraSupportPointsOnB, 4 ) ) +#endif + { + // Failed to create initial polyhedron + assert( false && "Failed to create initial polyhedron!" ); + return false; + } + + // Add initial faces to priority queue + +#ifdef _DEBUG + //m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); +#endif + + std::list< EpaFace* >& faces = m_polyhedron.GetFaces(); + + std::list< EpaFace* >::iterator facesItr( faces.begin() ); + + while ( facesItr != faces.end() ) + { + EpaFace* pFace = *facesItr; + + if ( !pFace->m_deleted ) + { +//#ifdef EPA_POLYHEDRON_USE_PLANES +// if ( pFace->m_planeDistance >= 0 ) +// { +// m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); +// assert( false && "Face's plane distance equal or greater than 0!" ); +// } +//#endif + + if ( pFace->IsAffinelyDependent() ) + { + assert( false && "One initial face is affinely dependent!" ); + return false; + } + + if ( pFace->m_vSqrd <= 0 ) + { + assert( false && "Face containing the origin!" ); + return false; + } + + if ( pFace->IsClosestPointInternal() ) + { + m_faceEntries.push_back( pFace ); + std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); + } + } + + ++facesItr; + } + +#ifdef _DEBUG + //m_polyhedron._dbgSaveToFile( "epa_start.dbg" ); +#endif + + assert( !m_faceEntries.empty() && "No faces added to heap!" ); + + return true; +} + +SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ) +{ + SimdVector3 v; + + SimdScalar upperBoundSqrd = SIMD_INFINITY; + SimdScalar vSqrd = 0; +#ifdef _DEBUG + SimdScalar prevVSqrd; +#endif + SimdScalar delta; + + bool isCloseEnough = false; + + EpaFace* pEpaFace = NULL; + + int nbIterations = 0; + //int nbMaxIterations = 1000; + + do + { + pEpaFace = m_faceEntries.front(); + std::pop_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); + m_faceEntries.pop_back(); + + if ( !pEpaFace->m_deleted ) + { +#ifdef _DEBUG + prevVSqrd = vSqrd; +#endif + + vSqrd = pEpaFace->m_vSqrd; + + if ( pEpaFace->m_planeDistance >= 0 ) + { + v = pEpaFace->m_planeNormal; + } + else + { + v = pEpaFace->m_v; + } + +#ifdef _DEBUG + //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); + assert( ( vSqrd >= prevVSqrd ) && "vSqrd decreased!" ); +#endif //_DEBUG + assert( ( v.length2() > 0 ) && "Zero vector not allowed!" ); + + SimdVector3 seperatingAxisInA = v * m_transformA.getBasis(); + SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis(); + + SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA ); + SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB ); + + SimdPoint3 pWorld = m_transformA( p ); + SimdPoint3 qWorld = m_transformB( q ); + + SimdPoint3 w = pWorld - qWorld; + delta = v.dot( w ); + + // Keep tighest upper bound + upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd ); + //assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" ); + + isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd ); + + if ( !isCloseEnough ) + { + std::list< EpaFace* > newFaces; + bool expandOk = m_polyhedron.Expand( w, pWorld, qWorld, pEpaFace, newFaces ); + + if ( expandOk ) + { + assert( !newFaces.empty() && "EPA polyhedron not expanding ?" ); + + bool check = true; + bool areEqual = false; + + while ( !newFaces.empty() ) + { + EpaFace* pNewFace = newFaces.front(); + assert( !pNewFace->m_deleted && "New face is deleted!" ); + + if ( !pNewFace->m_deleted ) + { + assert( ( pNewFace->m_vSqrd > 0 ) && "Face containing the origin!" ); + assert( !pNewFace->IsAffinelyDependent() && "Face is affinely dependent!" ); + +//#ifdef EPA_POLYHEDRON_USE_PLANES +//// if ( pNewFace->m_planeDistance >= 0 ) +//// { +// // assert( false && "Face's plane distance greater than 0!" ); +//#ifdef _DEBUG +//// m_polyhedron._dbgSaveToFile( "epa_beforeFix.dbg" ); +//#endif +// //pNewFace->FixOrder(); +//#ifdef _DEBUG +// //m_polyhedron._dbgSaveToFile( "epa_afterFix.dbg" ); +//#endif +//// } +//#endif +// +//#ifdef EPA_POLYHEDRON_USE_PLANES +// //assert( ( pNewFace->m_planeDistance < 0 ) && "Face's plane distance equal or greater than 0!" ); +//#endif + + if ( pNewFace->IsClosestPointInternal() && ( vSqrd <= pNewFace->m_vSqrd ) && ( pNewFace->m_vSqrd <= upperBoundSqrd ) ) + { + m_faceEntries.push_back( pNewFace ); + std::push_heap( m_faceEntries.begin(), m_faceEntries.end(), CompareEpaFaceEntries ); + } + } + + newFaces.pop_front(); + } + } + else + { + pEpaFace->CalcClosestPointOnA( wWitnessOnA ); + pEpaFace->CalcClosestPointOnB( wWitnessOnB ); + +#ifdef _DEBUG + //m_polyhedron._dbgSaveToFile( "epa_end.dbg" ); +#endif + + return v.length(); + } + } + } + + ++nbIterations; + } + while ( ( m_polyhedron.GetNbFaces() < EPA_MAX_FACE_ENTRIES ) &&/*( nbIterations < nbMaxIterations ) &&*/ + !isCloseEnough && ( m_faceEntries.size() > 0 ) && ( m_faceEntries[ 0 ]->m_vSqrd <= upperBoundSqrd ) ); + +#ifdef _DEBUG + //m_polyhedron._dbgSaveToFile( "epa_end.dbg" ); +#endif + + assert( pEpaFace && "Invalid epa face!" ); + + pEpaFace->CalcClosestPointOnA( wWitnessOnA ); + pEpaFace->CalcClosestPointOnB( wWitnessOnB ); + + return v.length(); +} + +bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1, + const SimdPoint3& point2, const SimdPoint3& point3 ) +{ + SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ), + ( point2 - point1 ).cross( point3 - point1 ), + ( point3 - point2 ).cross( point0 - point2 ), + ( point0 - point3 ).cross( point1 - point3 ) }; + + return ( ( facesNormals[ 0 ].dot( point0 ) > 0 ) != ( facesNormals[ 0 ].dot( point3 ) > 0 ) ) && + ( ( facesNormals[ 1 ].dot( point1 ) > 0 ) != ( facesNormals[ 1 ].dot( point0 ) > 0 ) ) && + ( ( facesNormals[ 2 ].dot( point2 ) > 0 ) != ( facesNormals[ 2 ].dot( point1 ) > 0 ) ) && + ( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) ); +} + +bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints ) +{ + return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] ); +} + +bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB ) +{ + return ( pFaceA->m_vSqrd > pFaceB->m_vSqrd ); +} diff --git a/Bullet/NarrowPhaseCollision/Epa.h b/Bullet/NarrowPhaseCollision/Epa.h new file mode 100644 index 000000000..43ecefaa2 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Epa.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_H +#define EPA_H + +#define EPA_MAX_FACE_ENTRIES 256 + +extern const SimdScalar EPA_MAX_RELATIVE_ERROR; +extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD; + +class Epa +{ + private : + + //! Prevents copying objects from this class + Epa( const Epa& epa ); + const Epa& operator = ( const Epa& epa ); + + public : + + Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB, + const SimdTransform& transformA, const SimdTransform& transformB ); + ~Epa(); + + bool Initialize( SimplexSolverInterface& simplexSolver ); + + SimdScalar CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ); + + private : + + bool TetrahedronContainsOrigin( SimdPoint3* pPoints ); + bool TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1, + const SimdPoint3& point2, const SimdPoint3& point3 ); + + private : + + //! Priority queue + std::vector< EpaFace* > m_faceEntries; + + ConvexShape* m_pConvexShapeA; + ConvexShape* m_pConvexShapeB; + + SimdTransform m_transformA; + SimdTransform m_transformB; + + EpaPolyhedron m_polyhedron; +}; + +extern bool CompareEpaFaceEntries( EpaFace* pFaceA, EpaFace* pFaceB ); + +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaCommon.h b/Bullet/NarrowPhaseCollision/EpaCommon.h new file mode 100644 index 000000000..9f0681aee --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaCommon.h @@ -0,0 +1,25 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_COMMON_H +#define EPA_COMMON_H + +#define EPA_POLYHEDRON_USE_PLANES + +//#define EPA_USE_HYBRID + +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaFace.cpp b/Bullet/NarrowPhaseCollision/EpaFace.cpp new file mode 100644 index 000000000..1e4ccf3fe --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaFace.cpp @@ -0,0 +1,254 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "SimdScalar.h" +#include "SimdVector3.h" +#include "SimdPoint3.h" + +#include "NarrowPhaseCollision/EpaCommon.h" + +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" + +#ifdef EPA_POLYHEDRON_USE_PLANES +SimdScalar PLANE_THICKNESS = 1e-5f; +#endif + +EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false ) +{ + m_pVertices[ 0 ] = m_pVertices[ 1 ] = m_pVertices[ 2 ] = 0; +} + +EpaFace::~EpaFace() +{ +} + +bool EpaFace::Initialize() +{ + assert( m_pHalfEdge && "Must setup half-edge first!" ); + + CollectVertices( m_pVertices ); + + const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; + const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; + + const SimdScalar e0Sqrd = e0.length2(); + const SimdScalar e1Sqrd = e1.length2(); + const SimdScalar e0e1 = e0.dot( e1 ); + + m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1; + + const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point ); + const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point ); + + m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0; + m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0; + + if ( IsAffinelyDependent() ) + { + return false; + } + + CalcClosestPoint(); + +#ifdef EPA_POLYHEDRON_USE_PLANES + if ( !CalculatePlane() ) + { + return false; + } +#endif + + return true; +} + +#ifdef EPA_POLYHEDRON_USE_PLANES +bool EpaFace::CalculatePlane() +{ + assert( ( m_pVertices[ 0 ] && m_pVertices[ 1 ] && m_pVertices[ 2 ] ) + && "Must setup vertices pointers first!" ); + + // Traditional method + + const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; + const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; + + m_planeNormal = v2.cross( v1 ); + + if ( m_planeNormal.length2() == 0 ) + { + return false; + } + + m_planeNormal.normalize(); + + m_planeDistance = m_pVertices[ 0 ]->m_point.dot( -m_planeNormal ); + + // Robust method + + //SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; + //SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; + + //SimdVector3 n; + + //n = _v2.cross( _v1 ); + + //_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 1 ]->m_point; + //_v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 1 ]->m_point; + + //n += ( _v1.cross( _v2 ) ); + + //_v1 = m_pVertices[ 0 ]->m_point - m_pVertices[ 2 ]->m_point; + //_v2 = m_pVertices[ 1 ]->m_point - m_pVertices[ 2 ]->m_point; + + //n += ( _v2.cross( _v1 ) ); + + //n /= 3; + //n.normalize(); + + //SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3; + //SimdScalar d = c.dot( -n ); + + //m_robustPlaneNormal = n; + //m_robustPlaneDistance = d; + + // Compare results from both methods and check whether they disagree + + //if ( d < 0 ) + //{ + // assert( ( m_planeDistance < 0 ) && "He he! Busted!" ); + //} + //else + //{ + // assert( ( m_planeDistance >= 0 ) && "He he! Busted!" ); + //} + + return true; +} +#endif + +void EpaFace::CalcClosestPoint() +{ + const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point; + const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point; + + m_v = m_pVertices[ 0 ]->m_point + + ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant; + + m_vSqrd = m_v.length2(); +} + +void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA ) +{ + const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA; + const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA; + + closestPointOnA = m_pVertices[ 0 ]->m_wSupportPointOnA + + ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / + m_determinant; +} + +void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB ) +{ + const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB; + const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB; + + closestPointOnB = m_pVertices[ 0 ]->m_wSupportPointOnB + + ( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / + m_determinant; +} + +bool EpaFace::IsAffinelyDependent() const +{ + return ( m_determinant <= SIMD_EPSILON ); +} + +bool EpaFace::IsClosestPointInternal() const +{ + return ( ( m_lambdas[ 0 ] >= 0 ) && ( m_lambdas[ 1 ] >= 0 ) && ( ( m_lambdas[ 0 ] + m_lambdas[ 1 ] <= m_determinant ) ) ); +} + +void EpaFace::CollectVertices( EpaVertex** ppVertices ) +{ + assert( m_pHalfEdge && "Invalid half-edge pointer!" ); + + int vertexIndex = 0; + + EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge; + + do + { + assert( ( ( vertexIndex >= 0 ) && ( vertexIndex < 3 ) ) && + "Face is not a triangle!" ); + + assert( pCurrentHalfEdge->m_pVertex && "Half-edge has an invalid vertex pointer!" ); + + ppVertices[ vertexIndex++ ] = pCurrentHalfEdge->m_pVertex; + + pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW; + + } + while( pCurrentHalfEdge != m_pHalfEdge ); +} + +//void EpaFace::FixOrder() +//{ +// EpaHalfEdge* pHalfEdges[ 3 ]; +// +// int halfEdgeIndex = 0; +// +// EpaHalfEdge* pCurrentHalfEdge = m_pHalfEdge; +// +// do +// { +// assert( ( ( halfEdgeIndex >= 0 ) && ( halfEdgeIndex < 3 ) ) && +// "Face is not a triangle!" ); +// +// pHalfEdges[ halfEdgeIndex++ ] = pCurrentHalfEdge; +// +// pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW; +// } +// while( pCurrentHalfEdge != m_pHalfEdge ); +// +// EpaVertex* pVertices[ 3 ] = { pHalfEdges[ 0 ]->m_pVertex, +// pHalfEdges[ 1 ]->m_pVertex, +// pHalfEdges[ 2 ]->m_pVertex }; +// +// // Make them run in the opposite direction +// pHalfEdges[ 0 ]->m_pNextCCW = pHalfEdges[ 2 ]; +// pHalfEdges[ 1 ]->m_pNextCCW = pHalfEdges[ 0 ]; +// pHalfEdges[ 2 ]->m_pNextCCW = pHalfEdges[ 1 ]; +// +// // Make half-edges point to their correct origin vertices +// +// pHalfEdges[ 1 ]->m_pVertex = pVertices[ 2 ]; +// pHalfEdges[ 2 ]->m_pVertex = pVertices[ 0 ]; +// pHalfEdges[ 0 ]->m_pVertex = pVertices[ 1 ]; +// +// // Make vertices point to the correct half-edges +// +// //pHalfEdges[ 0 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 0 ]; +// //pHalfEdges[ 1 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 1 ]; +// //pHalfEdges[ 2 ]->m_pVertex->m_pHalfEdge = pHalfEdges[ 2 ]; +// +// // Flip normal and change the sign of plane distance +// +//#ifdef EPA_POLYHEDRON_USE_PLANES +// m_planeNormal = -m_planeNormal; +// m_planeDistance = -m_planeDistance; +//#endif +//} + diff --git a/Bullet/NarrowPhaseCollision/EpaFace.h b/Bullet/NarrowPhaseCollision/EpaFace.h new file mode 100644 index 000000000..5c2ffbde3 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaFace.h @@ -0,0 +1,83 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_FACE_H +#define EPA_FACE_H + +class EpaVertex; +class EpaHalfEdge; + +#ifdef EPA_POLYHEDRON_USE_PLANES +extern SimdScalar PLANE_THICKNESS; +#endif + +//! Note : This class is not supposed to be a base class +class EpaFace +{ + private : + + //! Prevents copying objects from this class + EpaFace( const EpaFace& epaFace ); + const EpaFace& operator = ( const EpaFace& epaFace ); + + public : + + EpaFace(); + ~EpaFace(); + + bool Initialize(); + +#ifdef EPA_POLYHEDRON_USE_PLANES + bool CalculatePlane(); +#endif + void CalcClosestPoint(); + void CalcClosestPointOnA( SimdVector3& closestPointOnA ); + void CalcClosestPointOnB( SimdVector3& closestPointOnB ); + + bool IsAffinelyDependent() const; + bool IsClosestPointInternal() const; + + void CollectVertices( EpaVertex** ppVertices ); + + //void FixOrder(); + + public : + + EpaHalfEdge* m_pHalfEdge; + + // We keep vertices here so we don't need to call CollectVertices + // every time we need them + EpaVertex* m_pVertices[ 3 ]; + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdVector3 m_planeNormal; + SimdScalar m_planeDistance; + + //SimdVector3 m_robustPlaneNormal; + //SimdScalar m_robustPlaneDistance; +#endif + + SimdVector3 m_v; + SimdScalar m_vSqrd; + + SimdScalar m_determinant; + SimdScalar m_lambdas[ 2 ]; + + bool m_deleted; +}; + +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaHalfEdge.h b/Bullet/NarrowPhaseCollision/EpaHalfEdge.h new file mode 100644 index 000000000..db38356c2 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaHalfEdge.h @@ -0,0 +1,58 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_HALF_EDGE_H +#define EPA_HALF_EDGE_H + +class EpaFace; +class EpaVertex; + +//! Note : This class is not supposed to be a base class +class EpaHalfEdge +{ + private : + + //! Prevents copying objects from this class + EpaHalfEdge( const EpaHalfEdge& epaHalfEdge ); + const EpaHalfEdge& operator = ( const EpaHalfEdge& epaHalfEdge ); + + public : + + EpaHalfEdge() : m_pTwin( 0 ), m_pNextCCW( 0 ), m_pFace( 0 ), m_pVertex( 0 ) + { + } + + ~EpaHalfEdge() + { + } + + public : + + //! Twin half-edge link + EpaHalfEdge* m_pTwin; + + //! Next half-edge in counter clock-wise ( CCW ) order + EpaHalfEdge* m_pNextCCW; + + //! Parent face link + EpaFace* m_pFace; + + //! Origin vertex link + EpaVertex* m_pVertex; +}; + +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp b/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp new file mode 100644 index 000000000..b6fc8737e --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.cpp @@ -0,0 +1,202 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "SimdScalar.h" +#include "SimdVector3.h" +#include "SimdPoint3.h" +#include "SimdTransform.h" +#include "SimdMinMax.h" + +#include + +#include "CollisionShapes/ConvexShape.h" + +#include "NarrowPhaseCollision/SimplexSolverInterface.h" + +#include "NarrowPhaseCollision/EpaCommon.h" + +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" +#include "NarrowPhaseCollision/EpaPolyhedron.h" +#include "NarrowPhaseCollision/Epa.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" +#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" + +SimdScalar g_GJKMaxRelError = 1e-3f; +SimdScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError; + +bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, + class IDebugDraw* debugDraw ) +{ + assert( pConvexA && "Convex shape A is invalid!" ); + assert( pConvexB && "Convex shape B is invalid!" ); + + SimdScalar penDepth; + +#ifdef EPA_USE_HYBRID + bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB, + wWitnessOnA, wWitnessOnB, penDepth, v ); + + if ( needsEPA ) + { +#endif + penDepth = EpaPenDepth( simplexSolver, pConvexA, pConvexB, + transformA, transformB, + wWitnessOnA, wWitnessOnB ); + assert( ( penDepth > 0 ) && "EPA or Hybrid Technique failed to calculate penetration depth!" ); + +#ifdef EPA_USE_HYBRID + } +#endif + + return ( penDepth > 0 ); +} + +#ifdef EPA_USE_HYBRID +bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, + SimdScalar& penDepth, SimdVector3& v ) +{ + SimdScalar squaredDistance = SIMD_INFINITY; + SimdScalar delta = 0.f; + + const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin(); + const SimdScalar marginSqrd = margin * margin; + + simplexSolver.reset(); + + int nbIterations = 0; + + while ( true ) + { + assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); + + SimdVector3 seperatingAxisInA = -v * transformA.getBasis(); + SimdVector3 seperatingAxisInB = v * transformB.getBasis(); + + SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); + SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); + + SimdPoint3 pWorld = transformA( pInA ); + SimdPoint3 qWorld = transformB( qInB ); + + SimdVector3 w = pWorld - qWorld; + delta = v.dot( w ); + + // potential exit, they don't overlap + if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) ) + { + // Convex shapes do not overlap + // Returning true means that Hybrid's result is ok and there's no need to run EPA + penDepth = 0; + return true; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if ( ( squaredDistance - delta <= squaredDistance * g_GJKMaxRelErrorSqrd ) || simplexSolver.inSimplex( w ) ) + { + simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); + wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); + + penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; + + // Returning true means that Hybrid's result is ok and there's no need to run EPA + return true; + } + + //add current vertex to simplex + simplexSolver.addVertex( w, pWorld, qWorld ); + + //calculate the closest point to the origin (update vector v) + if ( !simplexSolver.closest( v ) ) + { + simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); + wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); + + penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; + + // Returning true means that Hybrid's result is ok and there's no need to run EPA + return true; + } + + SimdScalar previousSquaredDistance = squaredDistance; + squaredDistance = v.length2(); + + //are we getting any closer ? + if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) + { + simplexSolver.backup_closest( v ); + squaredDistance = v.length2(); + + simplexSolver.compute_points( wWitnessOnA, wWitnessOnB ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength ); + wWitnessOnB += v * ( pConvexB->GetMargin() / vLength ); + + penDepth = pConvexA->GetMargin() + pConvexB->GetMargin() - vLength; + + // Returning true means that Hybrid's result is ok and there's no need to run EPA + return true; + } + + if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) + { + // Convex Shapes intersect - we need to run EPA + // Returning false means that Hybrid couldn't do anything for us + // and that we need to run EPA to calculate the pen depth + return false; + } + + ++nbIterations; + } +} +#endif + +SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ) +{ + Epa epa( pConvexA, pConvexB, transformA, transformB ); + + if ( !epa.Initialize( simplexSolver ) ) + { + assert( false && "Epa failed to initialize!" ); + return 0; + } + + return epa.CalcPenDepth( wWitnessOnA, wWitnessOnB ); +} + diff --git a/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h b/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h new file mode 100644 index 000000000..8c06048ed --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaPenetrationDepthSolver.h @@ -0,0 +1,56 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_PENETRATION_DEPTH_H +#define EPA_PENETRATION_DEPTH_H + +/** +* EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to +* calculate the penetration depth between two convex shapes. +*/ + +extern SimdScalar g_GJKMaxRelError; +extern SimdScalar g_GJKMaxRelErrorSqrd; + +//! Note : This class is not supposed to be a base class +class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver +{ + public : + + bool CalcPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, + class IDebugDraw* debugDraw ); + + private : + +#ifdef EPA_USE_HYBRID + bool HybridPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB, + SimdScalar& penDepth, SimdVector3& v ); +#endif + + SimdScalar EpaPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* pConvexA, ConvexShape* pConvexB, + const SimdTransform& transformA, const SimdTransform& transformB, + SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB ); +}; + +#endif // EPA_PENETRATION_DEPTH_H + diff --git a/Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp b/Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp new file mode 100644 index 000000000..99a0857c7 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaPolyhedron.cpp @@ -0,0 +1,1014 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "SimdScalar.h" +#include "SimdVector3.h" +#include "SimdPoint3.h" +#include "Memory2.h" + +#include +#ifdef _DEBUG +#include +#endif + + +#include "NarrowPhaseCollision/EpaCommon.h" + +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" +#include "NarrowPhaseCollision/EpaPolyhedron.h" + + +EpaPolyhedron::EpaPolyhedron() : m_nbFaces( 0 ) +{ +} + +EpaPolyhedron::~EpaPolyhedron() +{ + Destroy(); +} + +bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints, + SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, + const int nbInitialPoints ) +{ +#ifndef EPA_POLYHEDRON_USE_PLANES + assert( ( nbInitialPoints <= 4 ) && "nbInitialPoints greater than 4!" ); +#endif + + if ( nbInitialPoints < 4 ) + { + // Insufficient nb of points + return false; + } + + //////////////////////////////////////////////////////////////////////////////// + +#ifdef EPA_POLYHEDRON_USE_PLANES + int nbDiffCoords[ 3 ] = { 0, 0, 0 }; + + bool* pDiffCoords = new bool[ 3 * nbInitialPoints ]; + + + int i; + for (i=0;i nbDiffCoords[ i + 1 ] ) + { + int tmp = nbDiffCoords[ i ]; + nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ]; + nbDiffCoords[ i + 1 ] = tmp; + + tmp = axisOrderIndices[ i ]; + axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ]; + axisOrderIndices[ i + 1 ] = tmp; + } + } + + int nbSuccessfullAxis = 0; + + // The axes with less different coordinates choose first + int minsIndices[ 3 ] = { -1, -1, -1 }; + int maxsIndices[ 3 ] = { -1, -1, -1 }; + + int finalPointsIndex = 0; + + for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis ) + { + int axisIndex = axisOrderIndices[ axis ]; + + SimdScalar axisMin = SIMD_INFINITY; + SimdScalar axisMax = -SIMD_INFINITY; + + for ( int i = 0; i < 4; ++i ) + { + // Among the diff coords pick the min and max coords + + if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] ) + { + if ( pInitialPoints[ i ][ axisIndex ] < axisMin ) + { + axisMin = pInitialPoints[ i ][ axisIndex ]; + minsIndices[ axisIndex ] = i; + } + + if ( pInitialPoints[ i ][ axisIndex ] > axisMax ) + { + axisMax = pInitialPoints[ i ][ axisIndex ]; + maxsIndices[ axisIndex ] = i; + } + } + } + + //assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) && + // "min and max have the same index!" ); + + if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) && + ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) ) + { + ++nbSuccessfullAxis; + + finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ]; + finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ]; + + // Make the choosen points to be impossible for other axes to choose + + //assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); + //assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" ); + + for ( int i = 0; i < 3; ++i ) + { + pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false; + pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false; + } + } + } + + if ( nbSuccessfullAxis <= 1 ) + { + // Degenerate input ? + assert( false && "nbSuccessfullAxis must be greater than 1!" ); + return false; + } + + delete[] pDiffCoords; +#endif + + ////////////////////////////////////////////////////////////////////////// + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; + SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ]; +#else + SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ]; + SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ]; +#endif + + SimdVector3 planeNormal = v1.cross( v0 ); + planeNormal.normalize(); + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal ); +#else + SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal ); +#endif + +#ifdef EPA_POLYHEDRON_USE_PLANES + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); + assert( SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS ) && + "Point should be on plane!" ); +#endif + +#ifndef EPA_POLYHEDRON_USE_PLANES + { + if ( planeDistance > 0 ) + { + SimdVector3 tmp = pInitialPoints[ 1 ]; + pInitialPoints[ 1 ] = pInitialPoints[ 2 ]; + pInitialPoints[ 2 ] = tmp; + + tmp = pSupportPointsOnA[ 1 ]; + pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ]; + pSupportPointsOnA[ 2 ] = tmp; + + tmp = pSupportPointsOnB[ 1 ]; + pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ]; + pSupportPointsOnB[ 2 ] = tmp; + } + } + + EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] ); + EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] ); + EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] ); + EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] ); +#else + finalPointsIndices[ 3 ] = -1; + + SimdScalar absMaxDist = -SIMD_INFINITY; + SimdScalar maxDist; + + for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex ) + { + SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance; + SimdScalar absDist = abs( dist ); + + if ( ( absDist > absMaxDist ) && + !SimdEqual( dist, PLANE_THICKNESS ) ) + { + absMaxDist = absDist; + maxDist = dist; + finalPointsIndices[ 3 ] = pointIndex; + } + } + + if ( finalPointsIndices[ 3 ] == -1 ) + { + Destroy(); + return false; + } + + if ( maxDist > PLANE_THICKNESS ) + { + // Can swap indices only + + SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ]; + pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ]; + pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp; + + tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ]; + pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ]; + pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp; + + tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ]; + pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ]; + pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp; + } + + EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] ); + EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] ); + EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] ); + EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] ); +#endif + + EpaFace* pFaceA = CreateFace(); + EpaFace* pFaceB = CreateFace(); + EpaFace* pFaceC = CreateFace(); + EpaFace* pFaceD = CreateFace(); + + EpaHalfEdge* pFaceAHalfEdges[ 3 ]; + EpaHalfEdge* pFaceCHalfEdges[ 3 ]; + EpaHalfEdge* pFaceBHalfEdges[ 3 ]; + EpaHalfEdge* pFaceDHalfEdges[ 3 ]; + + pFaceAHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceAHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceAHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceBHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceBHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceBHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceCHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceCHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceCHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceDHalfEdges[ 0 ] = CreateHalfEdge(); + pFaceDHalfEdges[ 1 ] = CreateHalfEdge(); + pFaceDHalfEdges[ 2 ] = CreateHalfEdge(); + + pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; + pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ]; + pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ]; + pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ]; + + pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ]; + pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ]; + pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ]; + + pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ]; + pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ]; + pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ]; + + pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ]; + pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ]; + pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ]; + + pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ]; + pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ]; + pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ]; + + + pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA; + pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA; + pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA; + + pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB; + pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB; + pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB; + + pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC; + pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC; + pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC; + + pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD; + pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD; + pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD; + + + pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA; + pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB; + pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB; + pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD; + pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD; + pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA; + pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC; + + pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB; + pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA; + pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD; + + //pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ]; + //pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ]; + //pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ]; + //pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ]; + + pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ]; + pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ]; + pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ]; + + pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ]; + pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ]; + pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ]; + + pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ]; + pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ]; + pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ]; + + pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ]; + pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ]; + pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ]; + + if ( !pFaceA->Initialize() || !pFaceB->Initialize() || + !pFaceC->Initialize() || !pFaceD->Initialize() ) + { + assert( false && "One initial face failed to initialize!" ); + return false; + } + +#ifdef EPA_POLYHEDRON_USE_PLANES + if ( nbInitialPoints > 4 ) + { + for ( int i = 0; i < nbInitialPoints; ++i ) + { + if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) && + ( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) ) + { + std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); + + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance; + + if ( dist > PLANE_THICKNESS ) + { + std::list< EpaFace* > newFaces; + + bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ], + pFace, newFaces ); + + if ( !expandOk ) + { + // One or more new faces are affinely dependent + return false; + } + + assert( !newFaces.empty() && "Polyhedron should have expanded!" ); + + break; + } + + ++facesItr; + } + } + } + } +#endif + + return true; +} + +void EpaPolyhedron::Destroy() +{ + DestroyAllVertices(); + + DestroyAllHalfEdges(); + + DestroyAllFaces(); +} + +EpaFace* EpaPolyhedron::CreateFace() +{ + EpaFace* pNewFace = new EpaFace(); + assert( pNewFace && "Failed to allocate memory for a new EpaFace!" ); + + m_faces.push_back( pNewFace ); + + ++m_nbFaces; + + return pNewFace; +} + +EpaHalfEdge* EpaPolyhedron::CreateHalfEdge() +{ + EpaHalfEdge* pNewHalfEdge = new EpaHalfEdge(); + assert( pNewHalfEdge && "Failed to allocate memory for a new EpaHalfEdge!" ); + + m_halfEdges.push_back( pNewHalfEdge ); + + return pNewHalfEdge; +} + +EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB ) +{ + EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); + assert( pNewVertex && "Failed to allocate memory for a new EpaVertex!" ); + + m_vertices.push_back( pNewVertex ); + + return pNewVertex; +} + +void EpaPolyhedron::DeleteFace( EpaFace* pFace ) +{ + pFace->m_deleted = true; + --m_nbFaces; +} + +void EpaPolyhedron::DestroyAllFaces() +{ + while ( !m_faces.empty() ) + { + EpaFace* pFace = m_faces.front(); + + delete pFace; + + m_faces.pop_front(); + } + + m_nbFaces = 0; +} + +void EpaPolyhedron::DestroyAllHalfEdges() +{ + while ( !m_halfEdges.empty() ) + { + EpaHalfEdge* pHalfEdge = m_halfEdges.front(); + + delete pHalfEdge; + + m_halfEdges.pop_front(); + } +} + +void EpaPolyhedron::DestroyAllVertices() +{ + while ( !m_vertices.empty() ) + { + EpaVertex* pVertex = m_vertices.front(); + + delete pVertex; + + m_vertices.pop_front(); + } +} + +bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB, + EpaFace* pFace, std::list< EpaFace* >& newFaces ) +{ + assert( !pFace->m_deleted && "Face is already deleted!" ); + assert( newFaces.empty() && "NewFaces list must be empty!" ); + + assert( !pFace->m_deleted && "Cannot expand deleted face!" ); + + // wSupportPoint must be front of face's plane used to do the expansion + +#ifdef EPA_POLYHEDRON_USE_PLANES + SimdScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance; + if ( dist <= PLANE_THICKNESS ) + { + return false; + } +#endif + + std::list< EpaHalfEdge* > coneBaseEdges; + DeleteVisibleFaces( wSupportPoint, pFace, coneBaseEdges ); + + assert( ( coneBaseEdges.size() >= 3 ) && "Cone base must have at least 3 edges!" ); + + EpaVertex* pConeAppex = CreateVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB ); + assert( pConeAppex && "Failed to create vertex!" ); + + CreateCone( pConeAppex, coneBaseEdges, newFaces ); + + // Initialize new faces + + std::list< EpaFace* >::iterator newFacesItr( newFaces.begin() ); + + while ( newFacesItr != newFaces.end() ) + { + EpaFace* pNewFace = *newFacesItr; + + if ( !pNewFace->Initialize() ) + { + return false; + } + + ++newFacesItr; + } + + return true; +} + +std::list< EpaFace* >& EpaPolyhedron::GetFaces() +{ + return m_faces; +} + +int EpaPolyhedron::GetNbFaces() const +{ + return m_faces.size(); +} + +void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, + std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ) +{ + assert( !pFace->m_deleted && "Face is already deleted!" ); + + DeleteFace( pFace ); + + EpaHalfEdge* pCurrentHalfEdge = pFace->m_pHalfEdge; + + do + { + assert( pCurrentHalfEdge->m_pTwin && "Half-edge without a twin!" ); + + EpaFace* pAdjacentFace = pCurrentHalfEdge->m_pTwin->m_pFace; + assert( pAdjacentFace && "Invalid adjacent face!" ); + + if ( !pAdjacentFace->m_deleted ) + { +#ifdef EPA_POLYHEDRON_USE_PLANES + assert( ( pAdjacentFace->m_planeNormal.length2() > 0 ) && "Invalid plane!" ); + + SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) + + pAdjacentFace->m_planeDistance; + + if ( pointDist > PLANE_THICKNESS ) +#else + SimdScalar dot = pAdjacentFace->m_v.dot( point ); + if ( dot >= pAdjacentFace->m_vSqrd ) +#endif + { + DeleteVisibleFaces( point, pAdjacentFace, coneBaseTwinHalfEdges ); + } + else + { + coneBaseTwinHalfEdges.push_back( pCurrentHalfEdge->m_pTwin ); + } + } + + pCurrentHalfEdge = pCurrentHalfEdge->m_pNextCCW; + } + while( pCurrentHalfEdge != pFace->m_pHalfEdge ); +} + +void EpaPolyhedron::CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, + std::list< EpaFace* >& newFaces ) +{ + assert( ( baseTwinHalfEdges.size() >= 3 ) && "DeleteVisibleFaces method didn't do its job right!" ); + + std::list< EpaHalfEdge* >::iterator baseHalfEdgesItr( baseTwinHalfEdges.begin() ); + std::list< EpaHalfEdge* > halfEdgesToLink; + + while ( baseHalfEdgesItr != baseTwinHalfEdges.end() ) + { + EpaFace* pNewFace = CreateConeFace( pAppexVertex, *baseHalfEdgesItr, halfEdgesToLink ); + + newFaces.push_back( pNewFace ); + + ++baseHalfEdgesItr; + } + + // Connect consecutive faces by linking twin half-edges + + assert( ( halfEdgesToLink.size() % 2 == 0 ) && "Nb half-edges to link is odd!" ); + + int nbLinksToCreate = halfEdgesToLink.size() / 2; + int nbLinksCreated = 0; + + std::list< EpaHalfEdge* >::iterator halfEdgesItr( halfEdgesToLink.begin() ); + + for ( ; ( halfEdgesItr != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr ) + { + std::list< EpaHalfEdge* >::iterator halfEdgesItr2( halfEdgesItr ); + ++halfEdgesItr2; + + for ( ; ( halfEdgesItr2 != halfEdgesToLink.end() ) && ( nbLinksCreated < nbLinksToCreate ); ++halfEdgesItr2 ) + { + EpaHalfEdge* pHalfEdge1 = *halfEdgesItr; + EpaHalfEdge* pHalfEdge2 = *halfEdgesItr2; + + EpaHalfEdge* pHalfEdgeNextCCW1 = pHalfEdge1->m_pNextCCW; + EpaHalfEdge* pHalfEdgeNextCCW2 = pHalfEdge2->m_pNextCCW; + + if ( ( pHalfEdge2->m_pVertex == pHalfEdgeNextCCW1->m_pVertex ) && + ( pHalfEdgeNextCCW2->m_pVertex == pHalfEdge1->m_pVertex ) ) + { + pHalfEdge1->m_pTwin = pHalfEdge2; + pHalfEdge2->m_pTwin = pHalfEdge1; + + ++nbLinksCreated; + } + } + } + + assert( ( nbLinksCreated == nbLinksToCreate ) && "Mesh topology not ok!" ); +} + +EpaFace* EpaPolyhedron::CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge, + std::list< EpaHalfEdge* >& halfEdgesToLink ) +{ + EpaFace* pNewFace = CreateFace(); + + EpaHalfEdge* pNewHalfEdge0 = CreateHalfEdge(); + EpaHalfEdge* pNewHalfEdge1 = CreateHalfEdge(); + EpaHalfEdge* pNewHalfEdge2 = CreateHalfEdge(); + + // Let new face point to one of its new half-edges + pNewFace->m_pHalfEdge = pNewHalfEdge0; + + // Link new half-edges in a loop + + pNewHalfEdge0->m_pNextCCW = pNewHalfEdge1; + pNewHalfEdge1->m_pNextCCW = pNewHalfEdge2; + pNewHalfEdge2->m_pNextCCW = pNewHalfEdge0; + + // Let new half-edges point to new face + + pNewHalfEdge0->m_pFace = pNewFace; + pNewHalfEdge1->m_pFace = pNewFace; + pNewHalfEdge2->m_pFace = pNewFace; + + // Let new half-edge 0 and base twin half-edge point to each other + + pNewHalfEdge0->m_pTwin = pBaseTwinHalfEdge; + pBaseTwinHalfEdge->m_pTwin = pNewHalfEdge0; + + // Let new half-edges know about their origin vertex + + pNewHalfEdge0->m_pVertex = pBaseTwinHalfEdge->m_pNextCCW->m_pVertex; + pNewHalfEdge1->m_pVertex = pBaseTwinHalfEdge->m_pVertex; + pNewHalfEdge2->m_pVertex = pAppexVertex; + + // Let vertices know about one of their outgoing edges + + //pNewHalfEdge0->m_pVertex->m_pHalfEdge = pNewHalfEdge0; + //pNewHalfEdge1->m_pVertex->m_pHalfEdge = pNewHalfEdge1; + //pNewHalfEdge2->m_pVertex->m_pHalfEdge = pNewHalfEdge2; + + halfEdgesToLink.push_back( pNewHalfEdge1 ); + halfEdgesToLink.push_back( pNewHalfEdge2 ); + + return pNewFace; +} + + +#ifdef DO_SOME_DEBUGGING_ +#ifdef _DEBUG +bool EpaPolyhedron::_dbgSaveToFile( const char* pFileName ) +{ + FILE* fp = NULL; + + if ( fopen_s( &fp, pFileName, "wb" ) != 0 ) + { + return false; + } + + unsigned long int nbBytesWritten; + unsigned long int byteIndex = 0; + + unsigned long int fileID = 0xBADC0DE; + fwrite( &fileID, sizeof( fileID ), 1, fp ); + nbBytesWritten = sizeof( fileID ); + byteIndex += nbBytesWritten; + + unsigned char reservedByte = 0; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + fwrite( &reservedByte, sizeof( reservedByte ), 1, fp ); + nbBytesWritten = sizeof( reservedByte ); + byteIndex += nbBytesWritten; + + unsigned char stringSize = 5; + fwrite( &stringSize, sizeof( stringSize ), 1, fp ); + nbBytesWritten = sizeof( stringSize ); + byteIndex += nbBytesWritten; + + char exportedFrom[ 6 ] = "01234"; + fwrite( exportedFrom, stringSize * sizeof( char ), 1, fp ); + nbBytesWritten = stringSize * sizeof( char ); + byteIndex += nbBytesWritten; + + unsigned short int w = 0; + + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + fwrite( &w, sizeof( w ), 1, fp ); + nbBytesWritten = sizeof( w ); + byteIndex += nbBytesWritten; + + unsigned long int geometryOffsetAtByteNb = byteIndex; + + unsigned long int geometryOffset = 0; + unsigned long int geometrySize = 0; + + fseek( fp, sizeof( geometryOffset ) + sizeof( geometrySize ), SEEK_CUR ); + byteIndex += sizeof( geometryOffset ) + sizeof( geometrySize ); + + unsigned long int mappingOffset = 0; + unsigned long int mappingSize = 0; + + fwrite( &mappingOffset, sizeof( mappingOffset ), 1, fp ); + nbBytesWritten = sizeof( mappingOffset ); + byteIndex += nbBytesWritten; + + fwrite( &mappingSize, sizeof( mappingSize ), 1, fp ); + nbBytesWritten = sizeof( mappingSize ); + byteIndex += nbBytesWritten; + + unsigned long int animationOffset = 0; + unsigned long int animationSize = 0; + + fwrite( &animationOffset, sizeof( animationOffset ), 1, fp ); + nbBytesWritten = sizeof( animationOffset ); + byteIndex += nbBytesWritten; + fwrite( &animationSize, sizeof( animationSize ), 1, fp ); + nbBytesWritten = sizeof( animationSize ); + byteIndex += nbBytesWritten; + + unsigned long int reservedDword = 0; + fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); + nbBytesWritten = sizeof( reservedDword ); + byteIndex += nbBytesWritten; + fwrite( &reservedDword, sizeof( reservedDword ), 1, fp ); + nbBytesWritten = sizeof( reservedDword ); + byteIndex += nbBytesWritten; + + geometryOffset = byteIndex; + + unsigned short int nbMeshs = 1; + fwrite( &nbMeshs, sizeof( nbMeshs ), 1, fp ); + nbBytesWritten = sizeof( nbMeshs ); + byteIndex += nbBytesWritten; + + char meshName[] = "noname mesh"; + unsigned char meshNameSize = strlen( meshName ); + + fwrite( &meshNameSize, sizeof( meshNameSize ), 1, fp ); + nbBytesWritten = sizeof( meshNameSize ); + byteIndex += nbBytesWritten; + + fwrite( meshName, meshNameSize * sizeof( char ), 1, fp ); + nbBytesWritten = meshNameSize * sizeof( char ); + byteIndex += nbBytesWritten; + + stdext::hash_map< unsigned long int, int > verticesMap; + typedef std::pair< unsigned long int, int > PR; + + int vertexIndex = 0; + + // Hash only vertices from faces that are not deleted + + std::list< EpaFace* >::iterator facesItr( m_faces.begin() ); + int nbFaces = 0; + + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + if ( !pFace->m_deleted ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 0 ], vertexIndex ) ); + ++vertexIndex; + } + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 1 ], vertexIndex ) ); + ++vertexIndex; + } + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); + if ( vertexItr == verticesMap.end() ) + { + verticesMap.insert( PR( ( unsigned long int ) pFace->m_pVertices[ 2 ], vertexIndex ) ); + ++vertexIndex; + } + + ++nbFaces; + } + + ++facesItr; + } + + unsigned long int nbVertices = verticesMap.size(); + fwrite( &nbVertices, sizeof( nbVertices ), 1, fp ); + nbBytesWritten = sizeof( nbVertices ); + byteIndex += nbBytesWritten; + + // Collect all safe vertices + + float* pVertices = new float[ verticesMap.size() * 3 ]; + + stdext::hash_map< unsigned long int, int >::iterator verticesItr( verticesMap.begin() ); + + while ( verticesItr != verticesMap.end() ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + PR pr = *verticesItr; + + vertexItr = verticesMap.find( pr.first ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + + EpaVertex* pVertex = ( EpaVertex* ) vertexItr->first; + + pVertices[ vertexItr->second * 3 ] = pVertex->m_point.x(); + pVertices[ vertexItr->second * 3 + 1 ] = pVertex->m_point.y(); + pVertices[ vertexItr->second * 3 + 2 ] = pVertex->m_point.z(); + + ++verticesItr; + } + + unsigned long int* pIndices = new unsigned long int[ nbFaces * 3 ]; + + facesItr = m_faces.begin(); + + int facesIndex = 0; + while ( facesItr != m_faces.end() ) + { + EpaFace* pFace = *facesItr; + + if ( !pFace->m_deleted ) + { + stdext::hash_map< unsigned long int, int >::const_iterator vertexItr; + + int verticesIndices[ 3 ]; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 0 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 0 ] = vertexItr->second; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 1 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 1 ] = vertexItr->second; + + vertexItr = verticesMap.find( ( unsigned long int ) pFace->m_pVertices[ 2 ] ); + assert( ( vertexItr != verticesMap.end() ) && "Vertex not found in hash table!" ); + verticesIndices[ 2 ] = vertexItr->second; + + pIndices[ facesIndex * 3 ] = verticesIndices[ 0 ]; + pIndices[ facesIndex * 3 + 1 ] = verticesIndices[ 1 ]; + pIndices[ facesIndex * 3 + 2 ] = verticesIndices[ 2 ]; + + ++facesIndex; + } + + ++facesItr; + } + + fwrite( &nbFaces, sizeof( nbFaces ), 1, fp ); + nbBytesWritten = sizeof( nbFaces ); + byteIndex += nbBytesWritten; + + bool hasSmoothingGroups = false; + fwrite( &hasSmoothingGroups, sizeof( hasSmoothingGroups ), 1, fp ); + nbBytesWritten = sizeof( hasSmoothingGroups ); + byteIndex += nbBytesWritten; + + fwrite( pVertices, verticesMap.size() * 3 * sizeof( float ), 1, fp ); + nbBytesWritten = verticesMap.size() * 3 * sizeof( float ); + byteIndex += nbBytesWritten; + + // write indices + fwrite( pIndices, nbFaces * 3 * sizeof( unsigned long int ), 1, fp ); + nbBytesWritten = nbFaces * 3 * sizeof( unsigned long int ); + byteIndex += nbBytesWritten; + + delete[] pVertices; + delete[] pIndices; + + geometrySize = byteIndex - geometryOffset; + + fseek( fp, geometryOffsetAtByteNb, SEEK_SET ); + + fwrite( &geometryOffset, sizeof( geometryOffset ), 1, fp ); + nbBytesWritten = sizeof( geometryOffset ); + byteIndex += nbBytesWritten; + fwrite( &geometrySize, sizeof( geometrySize ), 1, fp ); + nbBytesWritten = sizeof( geometrySize ); + byteIndex += nbBytesWritten; + + fseek( fp, byteIndex, SEEK_SET ); + + fclose( fp ); + + return true; +} +#endif +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaPolyhedron.h b/Bullet/NarrowPhaseCollision/EpaPolyhedron.h new file mode 100644 index 000000000..29a511981 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaPolyhedron.h @@ -0,0 +1,89 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_POLYHEDRON_H +#define EPA_POLYHEDRON_H + +class EpaFace; +class EpaVertex; + +//! Note : This class is not supposed to be a base class +class EpaPolyhedron +{ + private : + + //! Prevents copying objects from this class + EpaPolyhedron( const EpaPolyhedron& epaPolyhedron ); + const EpaPolyhedron& operator = ( const EpaPolyhedron& epaPolyhedron ); + + public : + + EpaPolyhedron(); + ~EpaPolyhedron(); + + bool Create( SimdPoint3* pInitialPoints, + SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB, + const int nbInitialPoints ); + void Destroy(); + + EpaFace* CreateFace(); + EpaHalfEdge* CreateHalfEdge(); + EpaVertex* CreateVertex( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB ); + + void DeleteFace( EpaFace* pFace ); + + void DestroyAllFaces(); + void DestroyAllHalfEdges(); + void DestroyAllVertices(); + + bool Expand( const SimdPoint3& wSupportPoint, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB, + EpaFace* pFace, std::list< EpaFace* >& newFaces ); + + std::list< EpaFace* >& GetFaces(); + int GetNbFaces() const; + + private : + + void DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace, + std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges ); + + void CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges, + std::list< EpaFace* >& newFaces ); + EpaFace* CreateConeFace( EpaVertex* pAppexVertex, EpaHalfEdge* pBaseTwinHalfEdge, + std::list< EpaHalfEdge* >& halfEdgesToLink ); + +#ifdef _DEBUG + public : + //! Please don't remove this method, it will help debugging if some problems arise in the future + bool _dbgSaveToFile( const char* pFileName ); +#endif + + private : + + //! This is the number of valid faces, m_faces list also contain deleted faces + int m_nbFaces; + + std::list< EpaFace* > m_faces; + std::list< EpaHalfEdge* > m_halfEdges; + std::list< EpaVertex* > m_vertices; +}; + +#endif + diff --git a/Bullet/NarrowPhaseCollision/EpaVertex.h b/Bullet/NarrowPhaseCollision/EpaVertex.h new file mode 100644 index 000000000..7d5cea373 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/EpaVertex.h @@ -0,0 +1,61 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +EPA Copyright (c) Ricardo Padrela 2006 + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef EPA_VERTEX_H +#define EPA_VERTEX_H + +class EpaHalfEdge; + +//! Note : This class is not supposed to be a base class +class EpaVertex +{ + private : + + //! Prevents copying objects from this class + EpaVertex( const EpaVertex& epaVertex ); + const EpaVertex& operator = ( const EpaVertex& epaVertex ); + + public : + + EpaVertex( const SimdPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point ) + { + } + + EpaVertex( const SimdPoint3& point, + const SimdPoint3& wSupportPointOnA, + const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ), + m_wSupportPointOnA( wSupportPointOnA ), + m_wSupportPointOnB( wSupportPointOnB ) + { + } + + ~EpaVertex() + { + } + + public : + + //! This is not necessary + //EpaHalfEdge* m_pHalfEdge; + + SimdPoint3 m_point; + + SimdPoint3 m_wSupportPointOnA; + SimdPoint3 m_wSupportPointOnB; +}; + +#endif + diff --git a/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp b/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp new file mode 100644 index 000000000..2b494e459 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/GjkConvexCast.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 "GjkConvexCast.h" +#include "CollisionShapes/SphereShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" +#include "GjkPairDetector.h" +#include "PointCollector.h" + + +GjkConvexCast::GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver) +:m_simplexSolver(simplexSolver), +m_convexA(convexA), +m_convexB(convexB) +{ +} + +bool GjkConvexCast::calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result) +{ + + + MinkowskiSumShape combi(m_convexA,m_convexB); + MinkowskiSumShape* convex = &combi; + + SimdTransform rayFromLocalA; + SimdTransform rayToLocalA; + + rayFromLocalA = fromA.inverse()* fromB; + rayToLocalA = toA.inverse()* toB; + + + SimdTransform trA,trB; + trA = SimdTransform(fromA); + trB = SimdTransform(fromB); + trA.setOrigin(SimdPoint3(0,0,0)); + trB.setOrigin(SimdPoint3(0,0,0)); + + convex->SetTransformA(trA); + convex->SetTransformB(trB); + + + + + float radius = 0.01f; + + SimdScalar lambda = 0.f; + SimdVector3 s = rayFromLocalA.getOrigin(); + SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin(); + SimdVector3 x = s; + SimdVector3 n; + n.setValue(0,0,0); + bool hasResult = false; + SimdVector3 c; + + float lastLambda = lambda; + + //first solution, using GJK + + //no penetration support for now, perhaps pass a pointer when we really want it + ConvexPenetrationDepthSolver* penSolverPtr = 0; + + SimdTransform identityTrans; + identityTrans.setIdentity(); + + SphereShape raySphere(0.0f); + raySphere.SetMargin(0.f); + + SimdTransform sphereTr; + sphereTr.setIdentity(); + sphereTr.setOrigin( rayFromLocalA.getOrigin()); + + result.DrawCoordSystem(sphereTr); + { + PointCollector pointCollector1; + GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + + GjkPairDetector::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) + { + SimdScalar dist; + dist = (c-x).length(); + if (dist < radius) + { + //penetration + lastLambda = 1.f; + } + + //not close enough + while (dist > radius) + { + + n = x - c; + SimdScalar 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); + PointCollector pointCollector; + GjkPairDetector gjk(&raySphere,convex,m_simplexSolver,penSolverPtr); + GjkPairDetector::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/Bullet/NarrowPhaseCollision/GjkConvexCast.h b/Bullet/NarrowPhaseCollision/GjkConvexCast.h new file mode 100644 index 000000000..c83ffc12f --- /dev/null +++ b/Bullet/NarrowPhaseCollision/GjkConvexCast.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 + +#include "SimdVector3.h" +#include "ConvexCast.h" +class ConvexShape; +class MinkowskiSumShape; +#include "SimplexSolverInterface.h" + +///GjkConvexCast performs a raycast on a convex object using support mapping. +class GjkConvexCast : public ConvexCast +{ + SimplexSolverInterface* m_simplexSolver; + ConvexShape* m_convexA; + ConvexShape* m_convexB; + +public: + + GjkConvexCast(ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver); + + /// cast a convex against another convex object + virtual bool calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result); + +}; + +#endif //GJK_CONVEX_CAST_H diff --git a/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp b/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp new file mode 100644 index 000000000..c1f4c4e47 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/GjkPairDetector.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 "GjkPairDetector.h" +#include "CollisionShapes/ConvexShape.h" +#include "NarrowPhaseCollision/SimplexSolverInterface.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" + +static const SimdScalar rel_error = SimdScalar(1.0e-5); +SimdScalar rel_error2 = rel_error * rel_error; +float maxdist2 = 1.e30f; + + + +GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver) +:m_cachedSeparatingAxis(0.f,0.f,1.f), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_minkowskiA(objectA), +m_minkowskiB(objectB), +m_ignoreMargin(false) +{ +} + +void GjkPairDetector::GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw) +{ + SimdScalar distance=0.f; + SimdVector3 normalInB(0.f,0.f,0.f); + SimdVector3 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; + } + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + + { + SimdScalar squaredDistance = SIMD_INFINITY; + SimdScalar delta = 0.f; + + SimdScalar margin = marginA + marginB; + + + + m_simplexSolver->reset(); + + while (true) + { + + SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); + SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + + SimdVector3 pInA = m_minkowskiA->LocalGetSupportingVertexWithoutMargin(seperatingAxisInA); + SimdVector3 qInB = m_minkowskiB->LocalGetSupportingVertexWithoutMargin(seperatingAxisInB); + SimdPoint3 pWorld = input.m_transformA(pInA); + SimdPoint3 qWorld = input.m_transformB(qInB); + + SimdVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > SimdScalar(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; + } + + SimdScalar 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 / SimdSqrt(lenSqr ); + normalInB *= rlen; //normalize + SimdScalar s = SimdSqrt(squaredDistance); + ASSERT(s > SimdScalar(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 /= SimdSqrt(lenSqr); + distance = -(pointOnA-pointOnB).length(); + } else + { + isValid = false; + } + } + } + } + } + + if (isValid) + { + output.AddContactPoint( + normalInB, + pointOnB, + distance); + //printf("gjk add:%f",distance); + } + + +} + + + + + diff --git a/Bullet/NarrowPhaseCollision/GjkPairDetector.h b/Bullet/NarrowPhaseCollision/GjkPairDetector.h new file mode 100644 index 000000000..c550728f8 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/GjkPairDetector.h @@ -0,0 +1,76 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "DiscreteCollisionDetectorInterface.h" +#include "SimdPoint3.h" + +#include + +class ConvexShape; +#include "SimplexSolverInterface.h" +class ConvexPenetrationDepthSolver; + +/// GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface +class GjkPairDetector : public DiscreteCollisionDetectorInterface +{ + + + SimdVector3 m_cachedSeparatingAxis; + ConvexPenetrationDepthSolver* m_penetrationDepthSolver; + SimplexSolverInterface* m_simplexSolver; + ConvexShape* m_minkowskiA; + ConvexShape* m_minkowskiB; + bool m_ignoreMargin; + +public: + + GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver); + virtual ~GjkPairDetector() {}; + + virtual void GetClosestPoints(const ClosestPointInput& input,Result& output,class IDebugDraw* debugDraw); + + void SetMinkowskiA(ConvexShape* minkA) + { + m_minkowskiA = minkA; + } + + void SetMinkowskiB(ConvexShape* minkB) + { + m_minkowskiB = minkB; + } + void SetCachedSeperatingAxis(const SimdVector3& seperatingAxis) + { + m_cachedSeparatingAxis = seperatingAxis; + } + + void SetPenetrationDepthSolver(ConvexPenetrationDepthSolver* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } + + void SetIgnoreMargin(bool ignoreMargin) + { + m_ignoreMargin = ignoreMargin; + } + +}; + +#endif //GJK_PAIR_DETECTOR_H diff --git a/Bullet/NarrowPhaseCollision/Hull.cpp b/Bullet/NarrowPhaseCollision/Hull.cpp new file mode 100644 index 000000000..35f3faa3f --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Hull.cpp @@ -0,0 +1,1080 @@ +// Bullet Continuous Collision Detection and Physics Library +// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +// +// +// Hull.cpp +// +// 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. +#ifdef WIN32 +#if _MSC_VER >= 1310 + +#include "hull.h" +#include "Geometry.h" +#include + +#include "HullContactCollector.h" + +Hull::Hull() +{ + m_numVerts = 0; + m_numFaces = 0; + m_numEdges = 0; + + m_pVerts = 0; + m_pFaces = 0; + m_pEdges = 0; + m_pPlanes = 0; +} + +Hull::~Hull() +{ + delete[] m_pVerts; + delete[] m_pFaces; + delete[] m_pEdges; + delete[] m_pPlanes; +} + +Point3 Hull::GetFaceCentroid(short face) const +{ + short edge; + + edge = GetFaceFirstEdge(face); + Vector3 c = Vector3(GetVertex(GetEdgeVertex0(face, edge))); + + for (edge = GetFaceNextEdge(face, edge); edge >= 0; edge = GetFaceNextEdge(face, edge)) + c += Vector3(GetVertex(GetEdgeVertex0(face, edge))); + + c /= Scalar(GetFace(face).m_numEdges); + + return Point3(c); +} + + +// helper stuff for MakeHull +short Hull::s_firstFreeTmpFace = 0; +short Hull::s_firstUsedTmpFace = 0; +Hull::TmpFace* Hull::s_pTmpFaces = 0; + +short Hull::s_firstFreeTmpEdge = 0; +short Hull::s_firstUsedTmpEdge = 0; +Hull::TmpEdge* Hull::s_pTmpEdges = 0; + +const Point3* Hull::s_pPoints = 0; + + +short Hull::AllocTmpFace() +{ + short face = s_firstFreeTmpFace; + assert(face != -1); + + TmpFace* pFace = GetTmpFace(face); + s_firstFreeTmpFace = pFace->m_next; + + pFace->m_next = s_firstUsedTmpFace; + s_firstUsedTmpFace = face; + + for (int i = 0; i < kTmpFaceMaxVerts; i++) + pFace->m_verts[i] = pFace->m_edges[i] = -1; + + pFace->m_plane = Maths::Zero; + + return face; +} + +void Hull::FreeTmpFace(short face) +{ + assert(face >= 0); + + TmpFace* pFace = GetTmpFace(face); + + if (face == s_firstUsedTmpFace) + s_firstUsedTmpFace = pFace->m_next; + else + { + TmpFace* pPrev; + for (pPrev = GetTmpFace(s_firstUsedTmpFace); pPrev->m_next != face; pPrev = GetTmpFace(pPrev->m_next)) + { + } + pPrev->m_next = pFace->m_next; + } + + pFace->m_next = s_firstFreeTmpFace; + s_firstFreeTmpFace = face; +} + +short Hull::AllocTmpEdge() +{ + short edge = s_firstFreeTmpEdge; + assert(edge != -1); + + TmpEdge* pEdge = GetTmpEdge(edge); + s_firstFreeTmpEdge = pEdge->m_next; + + pEdge->m_next = s_firstUsedTmpEdge; + s_firstUsedTmpEdge = edge; + + pEdge->m_verts[0] = pEdge->m_verts[1] = -1; + pEdge->m_faces[0] = pEdge->m_faces[1] = -1; + + return edge; +} + +void Hull::FreeTmpEdge(short edge) +{ + assert(edge >= 0); + + TmpEdge* pEdge = GetTmpEdge(edge); + + if (edge == s_firstUsedTmpEdge) + s_firstUsedTmpEdge = pEdge->m_next; + else + { + TmpEdge* pPrev; + for ( pPrev= GetTmpEdge(s_firstUsedTmpEdge); pPrev->m_next != edge; pPrev = GetTmpEdge(pPrev->m_next)) + { + } + pPrev->m_next = pEdge->m_next; + } + + pEdge->m_next = s_firstFreeTmpEdge; + s_firstFreeTmpEdge = edge; +} + +short Hull::MatchOrAddEdge(short vert0, short vert1, short face) +{ + assert(vert0 >= 0); + assert(vert1 >= 0); + assert(vert0 != vert1); + + TmpEdge* pEdge; + + // see if edge already exists + for (pEdge = GetTmpEdge(s_firstUsedTmpEdge); pEdge; pEdge = GetTmpEdge(pEdge->m_next)) + { + if (pEdge->m_verts[0] == vert0 && pEdge->m_verts[1] == vert1) + { + assert(pEdge->m_faces[0] == -1); + pEdge->m_faces[0] = face; + return pEdge->m_index; + } + + else if (pEdge->m_verts[0] == vert1 && pEdge->m_verts[1] == vert0) + { + assert(pEdge->m_faces[1] == -1); + pEdge->m_faces[1] = face; + return pEdge->m_index; + } + } + + // doesn't exist so add new face + short edge = AllocTmpEdge(); + assert(edge >= 0); + + pEdge = GetTmpEdge(edge); + + pEdge->m_verts[0] = vert0; + pEdge->m_verts[1] = vert1; + pEdge->m_faces[0] = face; + + return edge; +} + +void Hull::UnmatchOrRemoveEdge(short edge, short face) +{ + assert(edge >= 0); + TmpEdge* pEdge = GetTmpEdge(edge); + + if (pEdge->m_faces[0] == face) + { + pEdge->m_faces[0] = -1; + } + else + { + assert(pEdge->m_faces[1] == face); + pEdge->m_faces[1] = -1; + } + + // if edge is now redundant then free it + if (pEdge->m_faces[0] == -1 && pEdge->m_faces[1] == -1) + FreeTmpEdge(edge); +} + +short Hull::AddTmpFace(short vert0, short vert1, short vert2) +{ + short verts[3] = {vert0, vert1, vert2}; + return AddTmpFace(3, verts); +} + +short Hull::AddTmpFace(short vert0, short numOtherVerts, short* pVerts) +{ + short verts[256]; + verts[0] = vert0; + for (short i = 0; i < numOtherVerts; i++) + verts[i + 1] = pVerts[i]; + return AddTmpFace(numOtherVerts + 1, verts); +} + +short Hull::AddTmpFace(short numVerts, short* pVerts) +{ + assert(numVerts >= 3); + assert(numVerts <= kTmpFaceMaxVerts); + assert(pVerts); + + short face = AllocTmpFace(); + assert(face >= 0); + + TmpFace* pFace = GetTmpFace(face); + + pFace->m_numVerts = numVerts; + + for (short i = 0; i < numVerts; i++) + { + pFace->m_verts[i] = pVerts[i]; + pFace->m_edges[i] = MatchOrAddEdge(pVerts[i], pVerts[(i + 1) % numVerts], face); + } + + pFace->m_plane = Plane(s_pPoints[pVerts[0]], s_pPoints[pVerts[1]], s_pPoints[pVerts[2]]); + + return face; +} + +void Hull::RemoveTmpFace(short face) +{ + assert(face >= 0); + + TmpFace* pFace = GetTmpFace(face); + + for (short i = 0; i < pFace->m_numVerts; i++) + UnmatchOrRemoveEdge(pFace->m_edges[i], face); + + FreeTmpFace(face); +} + +bool Hull::TmpFaceAddPoint(short point, short face) +{ + assert(face >= 0); + + TmpFace* pFace = GetTmpFace(face); + + // vertex limit reached? + if (pFace->m_numVerts == kTmpFaceMaxVerts) + return false; + + // in same plane? + if (Abs(Dot(pFace->m_plane, s_pPoints[point])) > 0.001f) + return false; + + // remove last edge + UnmatchOrRemoveEdge(pFace->m_edges[pFace->m_numVerts - 1], face); + + // add 2 new edges + MatchOrAddEdge(pFace->m_verts[pFace->m_numVerts - 1], point, face); + MatchOrAddEdge(point, pFace->m_verts[0], face); + + // add new vertex + pFace->m_verts[pFace->m_numVerts++] = point; + + return true; +} + +// The resulting hull will not contain interior points +// Note that this is a cheap and cheerful implementation that can only handle +// reasonably small point sets. However, except for the final hull it doesn't do any dynamic +// memory allocation - all the work happens on the stack. +Hull* Hull::MakeHull(int numPoints, const Point3* pPoints) +{ + assert(numPoints >= 4); + assert(pPoints); + + // check first 4 points are disjoint + // TODO: make it search points so it can cope better with this + assert(Length(pPoints[1] - pPoints[0]) > 0.01f); + assert(Length(pPoints[2] - pPoints[0]) > 0.01f); + assert(Length(pPoints[3] - pPoints[0]) > 0.01f); + assert(Length(pPoints[2] - pPoints[1]) > 0.01f); + assert(Length(pPoints[3] - pPoints[1]) > 0.01f); + assert(Length(pPoints[3] - pPoints[2]) > 0.01f); + + s_pPoints = pPoints; + + // put all temp faces on a free list + TmpFace tmpFaces[kMaxFaces]; + + s_firstFreeTmpFace = 0; + s_firstUsedTmpFace = -1; + s_pTmpFaces = tmpFaces; + + for (short i = 0; i < kMaxEdges; i++) + { + tmpFaces[i].m_index = i; + tmpFaces[i].m_next = i + 1; + } + tmpFaces[kMaxFaces - 1].m_next = -1; + + // put all temp edges on a free list + TmpEdge tmpEdges[kMaxEdges]; + + s_firstFreeTmpEdge = 0; + s_firstUsedTmpEdge = -1; + s_pTmpEdges = tmpEdges; + + for (short i = 0; i < kMaxEdges; i++) + { + tmpEdges[i].m_index = i; + tmpEdges[i].m_next = i + 1; + } + tmpEdges[kMaxEdges - 1].m_next = -1; + + // make initial tetrahedron + Plane plane = Plane(pPoints[0], pPoints[1], pPoints[2]); + + Scalar dot = Dot(plane, pPoints[3]); + assert(Abs(dot) > 0.01f); // first 4 points are co-planar + + if (IsNegative(dot)) + { + AddTmpFace(0, 1, 2); + AddTmpFace(1, 0, 3); + AddTmpFace(0, 2, 3); + AddTmpFace(2, 1, 3); + } + else + { + AddTmpFace(2, 1, (short)0); + AddTmpFace(1, 2, 3); + AddTmpFace(2, 0, 3); + AddTmpFace(0, 1, 3); + } + + // merge all remaining points + for (int i = 4; i < numPoints; i++) + if (RemoveVisibleFaces(pPoints[i]) > 0) + FillHole(i); + + return MakeHullFromTemp(); +} + +int Hull::RemoveVisibleFaces(const Point3& point) +{ + // test point for containment in current hull + int numRemoved = 0; + TmpFace* pNextFace; + for (TmpFace* pFace = GetTmpFace(s_firstUsedTmpFace); pFace; pFace = pNextFace) + { + pNextFace = GetTmpFace(pFace->m_next); + + if (Dot(pFace->m_plane, point) > -0.001f) + { + RemoveTmpFace(pFace->m_index); + numRemoved++; + } + } + + return numRemoved; +} + +void Hull::FillHole(short newVertex) +{ + // gather unmatched edges (they form the silhouette of the hole) + short edgeVert0[kMaxEdges]; + short edgeVert1[kMaxEdges]; + int numEdges = 0; + + for (TmpEdge* pEdge = GetTmpEdge(s_firstUsedTmpEdge); pEdge; pEdge = GetTmpEdge(pEdge->m_next)) + { + if (pEdge->m_faces[0] == -1) + { + assert(numEdges < kMaxEdges); + edgeVert0[numEdges] = pEdge->m_verts[0]; + edgeVert1[numEdges] = pEdge->m_verts[1]; + numEdges++; + } + else if (pEdge->m_faces[1] == -1) + { + assert(numEdges < kMaxEdges); + edgeVert0[numEdges] = pEdge->m_verts[1]; + edgeVert1[numEdges] = pEdge->m_verts[0]; + numEdges++; + } + } + + // extract vertex winding by sorting edges + short verts[kMaxEdges + 1]; // +1 for repeat of first vertex + + verts[0] = edgeVert0[0]; + verts[1] = edgeVert1[0]; + short numVerts = 2; + + while (numVerts < (numEdges + 1)) + { + for (int i = 0; i < numEdges; i++) + { + // if leading vertex of edge matches our last vertex + if (edgeVert0[i] == verts[numVerts - 1]) + { + // then add trailing vertex of edge to our vertex list + verts[numVerts++] = edgeVert1[i]; + } + } + } + + // fill the hole + int i = 1; + while (i < numVerts) + { + // make plane for first 3 vertices + Plane plane = Plane(s_pPoints[newVertex], s_pPoints[verts[i - 1]], s_pPoints[verts[i]]); + + // any subequent vertices that lie in the same plane are also added + int numFaceVerts = 2; + for (int j = i + 1; j < numVerts; j++) + { + if (Abs(Dot(plane, s_pPoints[verts[j]])) > 0.001f) + break; + numFaceVerts++; + } + + // add the polygon + AddTmpFace(newVertex, numFaceVerts, verts + i - 1); + + // skip to next + i += numFaceVerts - 1; + } +} + +// make hull from temporary working structures +Hull* Hull::MakeHullFromTemp() +{ + Hull* pHull = new Hull(); + + // count and index faces + short index = 0; + + for (TmpFace* pFace = GetTmpFace(s_firstUsedTmpFace); pFace; pFace = GetTmpFace(pFace->m_next)) + { + pFace->m_index = index++; + pHull->m_numFaces++; + } + + // count and index edges, also count and index used vertices + short vertIndexes[kMaxVerts]; + for (int i = 0; i < kMaxVerts; i++) + vertIndexes[i] = -1; + + index = 0; + for (TmpEdge* pEdge = GetTmpEdge(s_firstUsedTmpEdge); pEdge; pEdge = GetTmpEdge(pEdge->m_next)) + { + pEdge->m_index = index++; + pHull->m_numEdges++; + + if (vertIndexes[pEdge->m_verts[0]] == -1) + vertIndexes[pEdge->m_verts[0]] = pHull->m_numVerts++; + if (vertIndexes[pEdge->m_verts[1]] == -1) + vertIndexes[pEdge->m_verts[1]] = pHull->m_numVerts++; + } + + // allocate hull arrays + pHull->m_pVerts = new Point3[pHull->m_numVerts]; + pHull->m_pEdges = new Hull::Edge[pHull->m_numEdges]; + pHull->m_pFaces = new Hull::Face[pHull->m_numFaces]; + pHull->m_pPlanes = new Plane[pHull->m_numFaces]; + + // fill in hull arrays + // verts: + for (int i = 0; i < kMaxVerts; i++) + if (vertIndexes[i] != -1) + pHull->m_pVerts[vertIndexes[i]] = s_pPoints[i]; + + // edges: + index = 0; + for (TmpEdge* pEdge = GetTmpEdge(s_firstUsedTmpEdge); pEdge; pEdge = GetTmpEdge(pEdge->m_next)) + { + pHull->m_pEdges[index].m_faces[0] = GetTmpFace(pEdge->m_faces[0])->m_index; + pHull->m_pEdges[index].m_faces[1] = GetTmpFace(pEdge->m_faces[1])->m_index; + pHull->m_pEdges[index].m_verts[0] = vertIndexes[pEdge->m_verts[0]]; + pHull->m_pEdges[index].m_verts[1] = vertIndexes[pEdge->m_verts[1]]; + + index++; + } + + // faces: + index = 0; + for (TmpFace* pFace = GetTmpFace(s_firstUsedTmpFace); pFace; pFace = GetTmpFace(pFace->m_next)) + { + // set edge count + pHull->m_pFaces[index].m_numEdges = pFace->m_numVerts; + + // make linked list of face edges + short prevEdge = GetTmpEdge(pFace->m_edges[0])->m_index; + pHull->m_pFaces[index].m_firstEdge = prevEdge; + + for (int i = 1; i < pFace->m_numVerts; i++) + { + Edge& e = pHull->m_pEdges[prevEdge]; + prevEdge = GetTmpEdge(pFace->m_edges[i])->m_index; + e.m_nextEdge[pFace->m_index == e.m_faces[1]] = prevEdge; + } + + Edge& e = pHull->m_pEdges[prevEdge]; + e.m_nextEdge[pFace->m_index == e.m_faces[1]] = -1; + + // set plane + pHull->m_pPlanes[index] = pFace->m_plane; + + index++; + } + + + return pHull; +} + + + +void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB,HullContactCollector* collector) +{ + Point3 vertsA[Hull::kMaxVerts]; + Point3 vertsB[Hull::kMaxVerts]; + +// const Hull& shapeA((const Hull&)*sep.m_pBodyA->GetShape()); +// const Hull& shapeB((const Hull&)*sep.m_pBodyB->GetShape()); + +// Transform trA(sep.m_pBodyA->GetTransform()); +// Transform trB(sep.m_pBodyB->GetTransform()); + + // transform verts of A to world space + Point3* pVertsA = vertsA; + for (short v = 0; v < shapeA.m_numVerts; v++) + pVertsA[v] = shapeA.m_pVerts[v] * trA; + + // transform verts of B to world space + Point3* pVertsB = vertsB; + for (short v = 0; v < shapeB.m_numVerts; v++) + pVertsB[v] = shapeB.m_pVerts[v] * trB; + +#ifdef SHAPE_COLLIDER_USE_CACHING + // update cached pair + if (sep.m_separator != Separation::kFeatureNone) + { + // re-use the separation + if (UpdateSeparationHullHull(sep, pVertsA, pVertsB, trA, trB) == true) + return; + + // also re-use penetration if only slight + if (sep.m_dist > -0.02f) + { + // except if no contacts were generated, in which case we continue through to full test + if (AddContactsHullHull(sep, pVertsA, pVertsB, trA, trB,shapeA,shapeB) > 0) + return; + } + } +#endif + + if (GetSeparationHullHull(sep, pVertsA, pVertsB, trA, trB,shapeA,shapeB) == false) + { + AddContactsHullHull(sep, pVertsA, pVertsB, trA, trB,shapeA,shapeB,collector); + } +} + + +void Hull::ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const +{ + assert(totalMass > 0.0f); + + // order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx + float integral[10] = {0,0,0,0,0,0,0,0,0,0}; + + // for each triangle + for (short face = 0; face < m_numFaces; face++) + { + short edge = GetFaceFirstEdge(face); + Point3 v0 = m_pVerts[ GetEdgeVertex0(face, edge) ] * transform; + + edge = GetFaceNextEdge(face, edge); + Point3 v1 = m_pVerts[ GetEdgeVertex0(face, edge) ] * transform; + + for (edge = GetFaceNextEdge(face, edge); edge != -1; edge = GetFaceNextEdge(face, edge)) + { + Point3 v2 = m_pVerts[ GetEdgeVertex0(face, edge) ] * transform; + + // get cross product of triangle edges + Vector3 d = Cross(v2 - v0, v1 - v0); + + // compute integral terms + Vector3 w0 = Vector3(v0); + Vector3 w1 = Vector3(v1); + Vector3 w2 = Vector3(v2); + + Vector3 temp0 = w0 + w1; + Vector3 f1 = temp0 + w2; + Vector3 temp1 = w0 * w0; + Vector3 temp2 = temp1 + w1 * temp0; + Vector3 f2 = temp2 + w2 * f1; + Vector3 f3 = w0 * temp1 + w1 * temp2 + w2 * f2; + Vector3 g0 = f2 + w0 * (f1 + w0); + Vector3 g1 = f2 + w1 * (f1 + w1); + Vector3 g2 = f2 + w2 * (f1 + w2); + + // update integrals + integral[0] += d[0] * f1[0]; + integral[1] += d[0] * f2[0]; + integral[2] += d[1] * f2[1]; + integral[3] += d[2] * f2[2]; + integral[4] += d[0] * f3[0]; + integral[5] += d[1] * f3[1]; + integral[6] += d[2] * f3[2]; + integral[7] += d[0] * (v0[1] * g0[0] + v1[1] * g1[0] + v2[1] * g2[0]); + integral[8] += d[1] * (v0[2] * g0[1] + v1[2] * g1[1] + v2[2] * g2[1]); + integral[9] += d[2] * (v0[0] * g0[2] + v1[0] * g1[2] + v2[0] * g2[2]); + + // next edge + v1 = v2; + } + } + + integral[0] *= 1.0f / 6.0f; + integral[1] *= 1.0f / 24.0f; + integral[2] *= 1.0f / 24.0f; + integral[3] *= 1.0f / 24.0f; + integral[4] *= 1.0f / 60.0f; + integral[5] *= 1.0f / 60.0f; + integral[6] *= 1.0f / 60.0f; + integral[7] *= 1.0f / 120.0f; + integral[8] *= 1.0f / 120.0f; + integral[9] *= 1.0f / 120.0f; + + // scale all integrals to get desired total mass + assert(integral[0] > 0.0f); + + float invMassRatio = totalMass / integral[0]; + for (int i = 0; i < 10; i++) + integral[i] *= invMassRatio; + + // center of mass + centerOfMass = Point3(integral[1] / totalMass, integral[2] / totalMass, integral[3] / totalMass); + + // inertia relative to world + inertia[0][0] = integral[5] + integral[6]; + inertia[0][1] = -integral[7]; + inertia[0][2] = -integral[9]; + + inertia[1][0] = -integral[7]; + inertia[1][1] = integral[4] + integral[6]; + inertia[1][2] = -integral[8]; + + inertia[2][0] = -integral[9]; + inertia[2][1] = -integral[8]; + inertia[2][2] = integral[5] + integral[5]; + + // inertia relative to center of mass + inertia[0][0] -= totalMass * (centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2]); + inertia[0][1] += totalMass * centerOfMass[0] * centerOfMass[1]; + inertia[0][2] += totalMass * centerOfMass[2] * centerOfMass[0]; + + inertia[1][0] += totalMass * centerOfMass[0] * centerOfMass[1]; + inertia[1][1] -= totalMass * (centerOfMass[2] * centerOfMass[2] + centerOfMass[0] * centerOfMass[0]); + inertia[1][2] += totalMass * centerOfMass[1] * centerOfMass[2]; + + inertia[2][0] += totalMass * centerOfMass[2] * centerOfMass[0]; + inertia[2][1] += totalMass * centerOfMass[1] * centerOfMass[2]; + inertia[2][2] -= totalMass * (centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1]); +} + +Bounds3 Hull::ComputeBounds(const Transform& transform) const +{ + Bounds3 b(m_pVerts[0] * transform); + for (int i = 1; i < m_numVerts; i++) + b += m_pVerts[i] * transform; + return b; +} + + + + + + + +// Clips a face to the back of a plane +int Hull::ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane) +{ + int ve, numVertsOut; + Point3 *pVtxOut, *pVtxS, *pVtxE; + Scalar ds, de; + + if (numVerts == 0) + return 0; + + pVtxE = *ppVtxIn; + pVtxS = pVtxE + numVerts - 1; + pVtxOut = *ppVtxOut; + + Scalar zero(0.0f); + + ds = Dot(plane, *pVtxS); + + for (ve = 0; ve < numVerts; ve++, pVtxE++) + { + de = Dot(plane, *pVtxE); + + if (ds <= zero) + { + *pVtxOut++ = *pVtxS; + if (de > zero) + *pVtxOut++ = Lerp(*pVtxS, *pVtxE, ds * RcpNr(ds - de)); + } + else if (de <= zero) + *pVtxOut++ = Lerp(*pVtxS, *pVtxE, ds * RcpNr(ds - de)); + + if (ve == 0) + pVtxS = *ppVtxIn; + else + pVtxS++; + + ds = de; + } + + numVertsOut = pVtxOut - *ppVtxOut; + + // swap in and out arrays ready for next time + pVtxOut = *ppVtxIn; + *ppVtxIn = *ppVtxOut; + *ppVtxOut = pVtxOut; + + return numVertsOut; +} + + + + + +int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, + const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB, + HullContactCollector* hullContactCollector) +{ + const int maxContacts = hullContactCollector->GetMaxNumContacts(); + + Vector3 normalWorld = sep.m_axis; + + // edge->edge contact is always a single point + if (sep.m_separator == Separation::kFeatureBoth) + { + const Hull::Edge& edgeA = hullA.GetEdge(sep.m_featureA); + const Hull::Edge& edgeB = hullB.GetEdge(sep.m_featureB); + + float ta, tb; + Line la(pVertsA[edgeA.m_verts[0]], pVertsA[edgeA.m_verts[1]]); + Line lb(pVertsB[edgeB.m_verts[0]], pVertsB[edgeB.m_verts[1]]); + + Intersect(la, lb, ta, tb); + +#ifdef VALIDATE_CONTACT_POINTS + AssertPointInsideHull(contact.m_points[0].m_pos, trA, hullA); + AssertPointInsideHull(contact.m_points[0].m_pos, trB, hullB); +#endif + + + Point3 posWorld = Lerp(la.m_start, la.m_end, ta); + float depth = -sep.m_dist; + Vector3 tangent = Normalize(pVertsA[edgeA.m_verts[1]] - pVertsA[edgeA.m_verts[0]]); + + sep.m_contact = hullContactCollector->BatchAddContactGroup(sep,1,normalWorld,tangent,&posWorld,&depth); + + } + // face->face contact is polygon + else + { + short faceA = sep.m_featureA; + short faceB = sep.m_featureB; + + Vector3 tangent; + + // find face of hull A that is most opposite contact axis + // TODO: avoid having to transform planes here + if (sep.m_separator == Separation::kFeatureB) + { + const Hull::Edge& edgeB = hullB.GetEdge(hullB.GetFaceFirstEdge(faceB)); + tangent = Normalize(pVertsB[edgeB.m_verts[1]] - pVertsB[edgeB.m_verts[0]]); + + Scalar dmin = Scalar::Consts::MaxValue; + for (short face = 0; face < hullA.m_numFaces; face++) + { + Vector3 normal = hullA.GetPlane(face).GetNormal() * trA; + Scalar d = Dot(normal, sep.m_axis); + if (d < dmin) + { + dmin = d; + faceA = face; + } + } + } + else + { + const Hull::Edge& edgeA = hullA.GetEdge(hullA.GetFaceFirstEdge(faceA)); + tangent = Normalize(pVertsA[edgeA.m_verts[1]] - pVertsA[edgeA.m_verts[0]]); + + Scalar dmin = Scalar::Consts::MaxValue; + for (short face = 0; face < hullB.m_numFaces; face++) + { + Vector3 normal = hullB.GetPlane(face).GetNormal() * trB; + Scalar d = Dot(normal, -sep.m_axis); + if (d < dmin) + { + dmin = d; + faceB = face; + } + } + } + + Point3 workspace[2][Hull::kMaxVerts]; + + // setup initial clip face (minimizing face from hull B) + int numContacts = 0; + for (short edge = hullB.GetFaceFirstEdge(faceB); edge != -1; edge = hullB.GetFaceNextEdge(faceB, edge)) + workspace[0][numContacts++] = pVertsB[ hullB.GetEdgeVertex0(faceB, edge) ]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + Point3* pVtxIn = workspace[0]; + Point3* pVtxOut = workspace[1]; + +#if 0 + for (short edge = hullA.GetFaceFirstEdge(faceA); edge != -1; edge = hullA.GetFaceNextEdge(faceA, edge)) + { + Plane planeA = hullA.GetPlane( hullA.GetEdgeOtherFace(edge, faceA) ) * trA; + numContacts = ClipFace(numContacts, &pVtxIn, &pVtxOut, planeA); + } +#else + for (short f = 0; f < hullA.GetNumFaces(); f++) + { + Plane planeA = hullA.GetPlane(f) * trA; + numContacts = ClipFace(numContacts, &pVtxIn, &pVtxOut, planeA); + } +#endif + + // only keep points that are behind the witness face + Plane planeA = hullA.GetPlane(faceA) * trA; + + float depths[Hull::kMaxVerts]; + int numPoints = 0; + for (int i = 0; i < numContacts; i++) + { + Scalar d = Dot(planeA, pVtxIn[i]); + if (IsNegative(d)) + { + depths[numPoints] = (float)-d; + pVtxIn[numPoints] = pVtxIn[i]; + +#ifdef VALIDATE_CONTACT_POINTS + AssertPointInsideHull(pVtxIn[numPoints], trA, hullA); + AssertPointInsideHull(pVtxIn[numPoints], trB, hullB); +#endif + numPoints++; + } + } + + //we can also use a persistentManifold/reducer class + // keep maxContacts points at most + if (numPoints > 0) + { + if (numPoints > maxContacts) + { + int step = (numPoints << 8) / maxContacts; + + numPoints = maxContacts; + for (int i = 0; i < numPoints; i++) + { + int nth = (step * i) >> 8; + + depths[i] = depths[nth]; + pVtxIn[i] = pVtxIn[nth]; + + +#ifdef VALIDATE_CONTACT_POINTS + AssertPointInsideHull(contact.m_points[i].m_pos, trA, hullA); + AssertPointInsideHull(contact.m_points[i].m_pos, trB, hullB); +#endif + } + } + + sep.m_contact = hullContactCollector->BatchAddContactGroup(sep,numPoints,normalWorld,tangent,pVtxIn,depths); + + } + return numPoints; + } + + // shut up compiler + return 0; +} + + + + +// returns true if a separating axis was found +// if no separating axis was found then details of least penetrating axis are returned +// resulting axis always points away from hullB +// either transform can be null in which case it is treated as identity (this avoids a bunch of work) +bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, + const Transform& trA, const Transform& trB, + const Hull& hullA, + const Hull& hullB + ) +{ + //const Hull& hullA((const Hull&)*sep.m_pShapeA->GetShape()); + //const Hull& hullB((const Hull&)*sep.m_pShapeB->GetShape()); + + sep.m_separator = Separation::kFeatureNone; + sep.m_dist = MinValueF; + sep.m_featureA = sep.m_featureB = -1; + sep.m_contact = -1; + + // test verts of A to planes of B + Scalar minDistB = Scalar::Consts::MinValue; + for (short p = 0; p < hullB.m_numFaces; p++) + { + Plane planeWorld = hullB.m_pPlanes[p] * trB; + + Scalar minDist = Dot(planeWorld, pVertsA[0]); + for (short v = 1; v < hullA.m_numVerts; v++) + minDist = Min(minDist, Dot(planeWorld, pVertsA[v])); + + // keep min overlap + if (minDist > minDistB) + { + minDistB = minDist; + sep.m_featureB = p; + sep.m_dist = minDist; + sep.m_axis = planeWorld.GetNormal(); + sep.m_separator = Separation::kFeatureB; + } + + // got a separating plane? + if (!IsNegative(minDist)) + return true; + } + + // test verts of B to planes of A + Scalar minDistA = Scalar::Consts::MinValue; + for (short p = 0; p < hullA.m_numFaces; p++) + { + // get plane in world space + Plane planeWorld = hullA.m_pPlanes[p] * trA; + + // get min dist + Scalar minDist = Dot(planeWorld, pVertsB[0]); + for (short v = 1; v < hullB.m_numVerts; v++) + minDist = Min(minDist, Dot(planeWorld, pVertsB[v])); + + // keep min overlap + if (minDist > minDistA) + { + minDistA = minDist; + sep.m_featureA = p; + + if ((float)minDist > sep.m_dist) + { + sep.m_dist = (float)minDist; + sep.m_axis = -planeWorld.GetNormal(); + sep.m_separator = Separation::kFeatureA; + } + } + + // got a separating plane? + if (!IsNegative(minDist)) + return true; + } + + // test edge pairs on two minimizing faces + short faceA( sep.m_featureA ); + short faceB( sep.m_featureB ); + + Vector3 faceBnormal = Vector3(hullB.m_pPlanes[sep.m_featureB]) * trB; + +// Plane bestPlane; +// Point3 bestPlanePos; + + for (short ea = hullA.GetFaceFirstEdge(faceA); ea != -1; ea = hullA.GetFaceNextEdge(faceA, ea)) + { + const Hull::Edge& edgeA = hullA.GetEdge(ea); + + for (short eb = hullB.GetFaceFirstEdge(faceB); eb != -1; eb = hullB.GetFaceNextEdge(faceB, eb)) + { + const Hull::Edge& edgeB = hullB.GetEdge(eb); + + Vector3 va = pVertsA[edgeA.m_verts[1]] - pVertsA[edgeA.m_verts[0]]; + Vector3 vb = pVertsB[edgeB.m_verts[1]] - pVertsB[edgeB.m_verts[0]]; + Vector3 axis = Cross(va, vb); + Scalar rcpLen = LengthRcp(axis); + + // if the two edges have nearly equal or opposite directions then try the next pair + if (IsNan(rcpLen) || rcpLen > 10000.0f) + continue; + axis *= rcpLen; + + // if axis is very close to current best axis then don't use it + if (Abs(Dot(sep.m_axis, axis)) > 0.99f) + continue; + + // ensure the axis points away from hullB + if (Dot(faceBnormal, axis) < Scalar::Consts::Zero) + axis = -axis; + + Plane plane(axis, pVertsB[edgeB.m_verts[0]]); + + // hull B must be entirely behind plane + Scalar dmaxB = Dot(plane, pVertsB[0]); + for (short v = 1; v < hullB.m_numVerts; v++) + dmaxB = Max(dmaxB, Dot(plane, pVertsB[v])); + + if (dmaxB > 0.001f) + continue; + + // get hull A distance from plane + Scalar dminA = Dot(plane, pVertsA[0]); + for (short v = 1; v < hullA.m_numVerts; v++) + dminA = Min(dminA, Dot(plane, pVertsA[v])); + + // got a separating plane? + if (!IsNegative(dminA)) + { + sep.m_featureA = ea; + sep.m_featureB = eb; + sep.m_dist = dminA; + sep.m_axis = axis; + sep.m_separator = Separation::kFeatureBoth; + return true; + } + + // keep min overlap + if (dminA > sep.m_dist) + { + // if edge of A is in front of the plane, then we haven't identified the correct edge pair + if (IsNegative(Dot(plane, pVertsA[edgeA.m_verts[0]]))) + { + // bestPlane = plane; + // bestPlanePos = Lerp(pVertsB[edgeB.m_verts[0]], pVertsB[edgeB.m_verts[1]], 0.5f); + +// sep.m_featureA = ea; +// sep.m_featureB = eb; + sep.m_dist = dminA; + sep.m_axis = axis; +// sep.m_separator = Separation::kFeatureBoth; + } + } + + } + +// RenderPlane(bestPlane, bestPlanePos); + } + + return false; +} +#endif //MSC_VER >= 1310 +#endif //WIN32 diff --git a/Bullet/NarrowPhaseCollision/Hull.h b/Bullet/NarrowPhaseCollision/Hull.h new file mode 100644 index 000000000..4dbbb4eb1 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Hull.h @@ -0,0 +1,174 @@ +// Bullet Continuous Collision Detection and Physics Library +// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +// +// +// Hull.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 SAT_HULL_H +#define SAT_HULL_H + +#include "Maths.h" +#include "Shape.h" + + +class DynWorld; +class HullContactCollector; + +/// Hull implements a convex collision detection algorithm based on Separating Axis Theorem (SAT). It is an alternative to GJK. +/// It calculates the separating axis, and based on that it calculates the contact manifold (points) in one go. +/// The separating axis calculation is approximated, not all edge-edge calculations are performed (performance reasons). +/// Future idea is to combine this with GJK for polyhedra: GJK to calculate the separating axis, and Hull clipping code to calculate the full set of contacts. +class Hull : public Shape +{ + friend class ShapeCollider; + +public: + struct Edge + { + short m_verts[2]; + short m_faces[2]; + short m_nextEdge[2]; // for each m_face + }; + + struct Face + { + short m_numEdges; + short m_firstEdge; + }; + +private: + static const int kMaxVerts = 256; + static const int kMaxFaces = 256; + static const int kMaxEdges = 256; + + short m_numVerts; + short m_numFaces; + short m_numEdges; + + Point3* m_pVerts; + Face* m_pFaces; + Edge* m_pEdges; + Plane* m_pPlanes; + + // hull construction stuff + static const int kTmpFaceMaxVerts = 64; + struct TmpFace + { + short m_index; + short m_next; + short m_numVerts; + short m_verts[kTmpFaceMaxVerts]; + short m_edges[kTmpFaceMaxVerts]; + Plane m_plane; + }; + + struct TmpEdge + { + short m_index; + short m_next; + short m_verts[2]; + short m_faces[2]; + }; + + static short s_firstFreeTmpFace; + static short s_firstUsedTmpFace; + static TmpFace* s_pTmpFaces; + + static short s_firstFreeTmpEdge; + static short s_firstUsedTmpEdge; + static TmpEdge* s_pTmpEdges; + + static const Point3* s_pPoints; + + static short AllocTmpFace(); + static void FreeTmpFace(short face); + static TmpFace* GetTmpFace(short index) {if (index < 0) return 0; return s_pTmpFaces + index;} + + static short AllocTmpEdge(); + static void FreeTmpEdge(short edge); + static TmpEdge* GetTmpEdge(short index) {if (index < 0) return 0; return s_pTmpEdges + index;} + + static short MatchOrAddEdge(short vert0, short vert1, short face); + static void UnmatchOrRemoveEdge(short edge, short face); + + static short AddTmpFace(short vert0, short vert1, short vert2); + static short AddTmpFace(short numVerts, short* pVerts); + static short AddTmpFace(short vert0, short numOtherVerts, short* pVerts); + static void RemoveTmpFace(short face); + + static bool TmpFaceAddPoint(short point, short face); + + static int RemoveVisibleFaces(const Point3& point); + static void FillHole(short newVertex); + static Hull* MakeHullFromTemp(); + +public: + Hull(); + ~Hull(); + +// ObjectType GetObjectType() const {return kTypeHull;} + + short GetNumVertices() const; + short GetNumFaces() const; + short GetNumEdges() const; + + const Point3& GetVertex(short index) const; + const Face& GetFace(short index) const; + const Edge& GetEdge(short index) const; + const Plane& GetPlane(short index) const; + + short GetFaceFirstEdge(short face) const; + short GetFaceNextEdge(short face, short prevEdge) const; + + short GetEdgeVertex0(short face, short edge) const; + short GetEdgeVertex1(short face, short edge) const; + + short GetEdgeOtherFace(short edge, short face) const; + + Point3 GetFaceCentroid(short face) const; + + //static void ProcessHullHull(Separation& sep); + static void ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector); + + virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const; + virtual Bounds3 ComputeBounds(const Transform& transform) const; + + static Hull* MakeHull(int numPoints, const Point3* pPoints); + + //for contact generation + + + + /// Clips a face to the back of a plane + static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane); + + static bool GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, + const Transform& trA, const Transform& trB, + const Hull& hullA, + const Hull& hullB + ); + + static int AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB, + const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB, + HullContactCollector* hullContactCollector); + + + +}; + + + +#include "hull.inl" + +#endif //SAT_HULL_H \ No newline at end of file diff --git a/Bullet/NarrowPhaseCollision/Hull.inl b/Bullet/NarrowPhaseCollision/Hull.inl new file mode 100644 index 000000000..912ec1774 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Hull.inl @@ -0,0 +1,100 @@ +// Bullet Continuous Collision Detection and Physics Library +// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +// +// +// Hull.inl +// +// 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. +#pragma once + +#include + +inline short Hull::GetNumVertices() const +{ + return m_numVerts; +} + +inline short Hull::GetNumFaces() const +{ + return m_numFaces; +} + +inline short Hull::GetNumEdges() const +{ + return m_numEdges; +} + +inline const Point3& Hull::GetVertex(short index) const +{ + return m_pVerts[index]; +} + +inline const Hull::Face& Hull::GetFace(short index) const +{ + return m_pFaces[index]; +} + +inline const Hull::Edge& Hull::GetEdge(short index) const +{ + return m_pEdges[index]; +} + +inline const Plane& Hull::GetPlane(short index) const +{ + return m_pPlanes[index]; +} + +inline short Hull::GetFaceFirstEdge(short face) const +{ + assert(face >= 0 && face < m_numFaces); + + return m_pFaces[face].m_firstEdge; +} + +inline short Hull::GetFaceNextEdge(short face, short prevEdge) const +{ + assert(face >= 0 && face < m_numFaces); + assert(prevEdge >= 0 && prevEdge < m_numEdges); + + const Edge& e = m_pEdges[prevEdge]; + return e.m_nextEdge[face == e.m_faces[1]]; +} + +inline short Hull::GetEdgeVertex0(short face, short edge) const +{ + assert(face >= 0 && face < m_numFaces); + assert(edge >= 0 && edge < m_numEdges); + + const Edge& e = m_pEdges[edge]; + return e.m_verts[face == e.m_faces[0]]; +} + +inline short Hull::GetEdgeVertex1(short face, short edge) const +{ + assert(face >= 0 && face < m_numFaces); + assert(edge >= 0 && edge < m_numEdges); + + const Edge& e = m_pEdges[edge]; + return e.m_verts[face == e.m_faces[1]]; +} + +inline short Hull::GetEdgeOtherFace(short edge, short face) const +{ + assert(face >= 0 && face < m_numFaces); + assert(edge >= 0 && edge < m_numEdges); + + const Edge& e = m_pEdges[edge]; + assert(e.m_faces[0] == face || e.m_faces[1] == face); + + return e.m_faces[face == e.m_faces[0]]; +} diff --git a/Bullet/NarrowPhaseCollision/HullContactCollector.h b/Bullet/NarrowPhaseCollision/HullContactCollector.h new file mode 100644 index 000000000..ba80f35a1 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/HullContactCollector.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 HULL_CONTACT_COLLECTOR_H +#define HULL_CONTACT_COLLECTOR_H + +class Vector3; +class Point3; +class Scalar; +struct Separation; + +///HullContactCollector collects the Hull computation to the contact point results +class HullContactCollector +{ +public: + + virtual ~HullContactCollector() {}; + + virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0; + + virtual int GetMaxNumContacts() const = 0; + +}; + +#endif //HULL_CONTACT_COLLECTOR_H \ No newline at end of file diff --git a/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp b/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp new file mode 100644 index 000000000..ad97b9579 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.cpp @@ -0,0 +1,48 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "ManifoldContactAddResult.h" +#include "NarrowPhaseCollision/PersistentManifold.h" + +ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr) + :m_manifoldPtr(manifoldPtr) +{ + m_transAInv = transA.inverse(); + m_transBInv = transB.inverse(); + +} + + +void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) +{ + if (depth > m_manifoldPtr->GetContactBreakingTreshold()) + return; + + + SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth; + SimdVector3 localA = m_transAInv(pointA ); + SimdVector3 localB = m_transBInv(pointInWorld); + ManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + + int insertIndex = m_manifoldPtr->GetCacheEntry(newPt); + if (insertIndex >= 0) + { + m_manifoldPtr->ReplaceContactPoint(newPt,insertIndex); + } else + { + m_manifoldPtr->AddManifoldPoint(newPt); + } +} + diff --git a/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h b/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.h new file mode 100644 index 000000000..5dc9f3129 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ManifoldContactAddResult.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 MANIFOLD_CONTACT_ADD_RESULT_H +#define MANIFOLD_CONTACT_ADD_RESULT_H + +#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h" +class PersistentManifold; + +class ManifoldContactAddResult : public DiscreteCollisionDetectorInterface::Result +{ + PersistentManifold* m_manifoldPtr; + SimdTransform m_transAInv; + SimdTransform m_transBInv; + +public: + + ManifoldContactAddResult(SimdTransform transA,SimdTransform transB,PersistentManifold* manifoldPtr); + + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth); + +}; + +#endif //MANIFOLD_CONTACT_ADD_RESULT_H diff --git a/Bullet/NarrowPhaseCollision/ManifoldPoint.h b/Bullet/NarrowPhaseCollision/ManifoldPoint.h new file mode 100644 index 000000000..35ebc4a40 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/ManifoldPoint.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 MANIFOLD_CONTACT_POINT_H +#define MANIFOLD_CONTACT_POINT_H + +#include "SimdVector3.h" +#include "SimdTransformUtil.h" + + + + + +/// ManifoldContactPoint collects and maintains persistent contactpoints. +/// used to improve stability and performance of rigidbody dynamics response. +class ManifoldPoint + { + public: + ManifoldPoint() + :m_userPersistentData(0) + { + } + + ManifoldPoint( const SimdVector3 &pointA, const SimdVector3 &pointB, + const SimdVector3 &normal, + SimdScalar distance ) : + m_localPointA( pointA ), + m_localPointB( pointB ), + m_normalWorldOnB( normal ), + m_distance1( distance ), + m_userPersistentData(0), + m_lifeTime(0) + { + + + } + + + + SimdVector3 m_localPointA; + SimdVector3 m_localPointB; + SimdVector3 m_positionWorldOnB; + ///m_positionWorldOnA is redundant information, see GetPositionWorldOnA(), but for clarity + SimdVector3 m_positionWorldOnA; + SimdVector3 m_normalWorldOnB; + + float m_distance1; + + + void* m_userPersistentData; + + int m_lifeTime;//lifetime of the contactpoint in frames + + float GetDistance() const + { + return m_distance1; + } + int GetLifeTime() const + { + return m_lifeTime; + } + + SimdVector3 GetPositionWorldOnA() { + return m_positionWorldOnA; +// return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + const SimdVector3& GetPositionWorldOnB() + { + return m_positionWorldOnB; + } + + void SetDistance(float dist) + { + m_distance1 = dist; + } + + + + }; + +#endif //MANIFOLD_CONTACT_POINT_H diff --git a/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp b/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp new file mode 100644 index 000000000..a5dabd423 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp @@ -0,0 +1,243 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "MinkowskiPenetrationDepthSolver.h" +#include "CollisionShapes/MinkowskiSumShape.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "NarrowPhaseCollision/GjkPairDetector.h" + + +struct MyResult : public DiscreteCollisionDetectorInterface::Result +{ + + MyResult():m_hasResult(false) + { + } + + SimdVector3 m_normalOnBInWorld; + SimdVector3 m_pointInWorld; + float m_depth; + bool m_hasResult; + + void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth) + { + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; + m_hasResult = true; + } +}; + +#define NUM_UNITSPHERE_POINTS 42 +static SimdVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS] = +{ +SimdVector3(0.000000f , -0.000000f,-1.000000f), +SimdVector3(0.723608f , -0.525725f,-0.447219f), +SimdVector3(-0.276388f , -0.850649f,-0.447219f), +SimdVector3(-0.894426f , -0.000000f,-0.447216f), +SimdVector3(-0.276388f , 0.850649f,-0.447220f), +SimdVector3(0.723608f , 0.525725f,-0.447219f), +SimdVector3(0.276388f , -0.850649f,0.447220f), +SimdVector3(-0.723608f , -0.525725f,0.447219f), +SimdVector3(-0.723608f , 0.525725f,0.447219f), +SimdVector3(0.276388f , 0.850649f,0.447219f), +SimdVector3(0.894426f , 0.000000f,0.447216f), +SimdVector3(-0.000000f , 0.000000f,1.000000f), +SimdVector3(0.425323f , -0.309011f,-0.850654f), +SimdVector3(-0.162456f , -0.499995f,-0.850654f), +SimdVector3(0.262869f , -0.809012f,-0.525738f), +SimdVector3(0.425323f , 0.309011f,-0.850654f), +SimdVector3(0.850648f , -0.000000f,-0.525736f), +SimdVector3(-0.525730f , -0.000000f,-0.850652f), +SimdVector3(-0.688190f , -0.499997f,-0.525736f), +SimdVector3(-0.162456f , 0.499995f,-0.850654f), +SimdVector3(-0.688190f , 0.499997f,-0.525736f), +SimdVector3(0.262869f , 0.809012f,-0.525738f), +SimdVector3(0.951058f , 0.309013f,0.000000f), +SimdVector3(0.951058f , -0.309013f,0.000000f), +SimdVector3(0.587786f , -0.809017f,0.000000f), +SimdVector3(0.000000f , -1.000000f,0.000000f), +SimdVector3(-0.587786f , -0.809017f,0.000000f), +SimdVector3(-0.951058f , -0.309013f,-0.000000f), +SimdVector3(-0.951058f , 0.309013f,-0.000000f), +SimdVector3(-0.587786f , 0.809017f,-0.000000f), +SimdVector3(-0.000000f , 1.000000f,-0.000000f), +SimdVector3(0.587786f , 0.809017f,-0.000000f), +SimdVector3(0.688190f , -0.499997f,0.525736f), +SimdVector3(-0.262869f , -0.809012f,0.525738f), +SimdVector3(-0.850648f , 0.000000f,0.525736f), +SimdVector3(-0.262869f , 0.809012f,0.525738f), +SimdVector3(0.688190f , 0.499997f,0.525736f), +SimdVector3(0.525730f , 0.000000f,0.850652f), +SimdVector3(0.162456f , -0.499995f,0.850654f), +SimdVector3(-0.425323f , -0.309011f,0.850654f), +SimdVector3(-0.425323f , 0.309011f,0.850654f), +SimdVector3(0.162456f , 0.499995f,0.850654f) +}; + + +bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver, + ConvexShape* convexA,ConvexShape* convexB, + const SimdTransform& transA,const SimdTransform& transB, + SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb, + class IDebugDraw* debugDraw + ) +{ + + //just take fixed number of orientation, and sample the penetration depth in that direction + float minProj = 1e30f; + SimdVector3 minNorm; + SimdVector3 minVertex; + SimdVector3 minA,minB; + SimdVector3 seperatingAxisInA,seperatingAxisInB; + SimdVector3 pInA,qInB,pWorld,qWorld,w; + +#define USE_BATCHED_SUPPORT 1 +#ifdef USE_BATCHED_SUPPORT + SimdVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 seperatingAxisInABatch[NUM_UNITSPHERE_POINTS]; + SimdVector3 seperatingAxisInBBatch[NUM_UNITSPHERE_POINTS]; + int i; + + for (i=0;iBatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,NUM_UNITSPHERE_POINTS); + convexB->BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,NUM_UNITSPHERE_POINTS); + for (i=0;iLocalGetSupportingVertexWithoutMargin(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) + { + SimdVector3 color(0,1,0); + debugDraw->DrawLine(minA,minB,color); + color = SimdVector3 (1,1,1); + SimdVector3 vec = minB-minA; + float prj2 = minNorm.dot(vec); + debugDraw->DrawLine(minA,minA+(minNorm*minProj),color); + + } +#endif //DEBUG_DRAW + + + + GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0); + + SimdScalar offsetDist = minProj; + SimdVector3 offset = minNorm * offsetDist; + + + + GjkPairDetector::ClosestPointInput input; + + SimdVector3 newOrg = transA.getOrigin() + offset; + + SimdTransform 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) + { + SimdVector3 color(1,0,0); + debugDraw->DrawLine(pa,pb,color); + } +#endif//DEBUG_DRAW + + + } + return res.m_hasResult; +} + + + diff --git a/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h b/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h new file mode 100644 index 000000000..b7a0da3a0 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.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 "ConvexPenetrationDepthSolver.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 MinkowskiPenetrationDepthSolver : public ConvexPenetrationDepthSolver +{ +public: + + virtual bool CalcPenDepth( SimplexSolverInterface& simplexSolver, + ConvexShape* convexA,ConvexShape* convexB, + const SimdTransform& transA,const SimdTransform& transB, + SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb, + class IDebugDraw* debugDraw + ); + +}; + +#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H + diff --git a/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/Bullet/NarrowPhaseCollision/PersistentManifold.cpp new file mode 100644 index 000000000..df72b9a04 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -0,0 +1,231 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "PersistentManifold.h" +#include "SimdTransform.h" +#include + +float gContactBreakingTreshold = 0.02f; +ContactDestroyedCallback gContactCallback = 0; + + +PersistentManifold::PersistentManifold() +:m_body0(0), +m_body1(0), +m_cachedPoints (0), +m_index1(0) +{ +} + + +void PersistentManifold::ClearManifold() +{ + int i; + for (i=0;i +void PersistentManifold::DebugPersistency() +{ + int i; + printf("DebugPersistency : numPoints %d\n",m_cachedPoints); + for (i=0;i1) + printf("error in ClearUserCache\n"); + } + } + assert(occurance<=0); +#endif //DEBUG_PERSISTENCY + + if (pt.m_userPersistentData && gContactCallback) + { + (*gContactCallback)(pt.m_userPersistentData); + pt.m_userPersistentData = 0; + } + +#ifdef DEBUG_PERSISTENCY + DebugPersistency(); +#endif + } + + +} + + + + +int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt) +{ + + //calculate 4 possible cases areas, and take biggest area + + SimdScalar res0,res1,res2,res3; + + { + SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + SimdVector3 cross = a0.cross(b0); + res0 = cross.length2(); + } + { + SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA; + SimdVector3 cross = a1.cross(b1); + res1 = cross.length2(); + } + { + SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 cross = a2.cross(b2); + res2 = cross.length2(); + } + { + SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA; + SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA; + SimdVector3 cross = a3.cross(b3); + res3 = cross.length2(); + } + + SimdVector4 maxvec(res0,res1,res2,res3); + int biggestarea = maxvec.closestAxis4(); + + return biggestarea; + + +} + + + +int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const +{ + SimdScalar shortestDist = GetContactBreakingTreshold() * GetContactBreakingTreshold(); + int size = GetNumContacts(); + int nearestPoint = -1; + for( int i = 0; i < size; i++ ) + { + const ManifoldPoint &mp = m_pointCache[i]; + + SimdVector3 diffA = mp.m_localPointA- newPoint.m_localPointA; + const SimdScalar distToManiPoint = diffA.dot(diffA); + if( distToManiPoint < shortestDist ) + { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; +} + +void PersistentManifold::AddManifoldPoint(const ManifoldPoint& 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 PersistentManifold::GetContactBreakingTreshold() const +{ + return gContactBreakingTreshold; +} + +void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB) +{ + int i; + + /// first refresh worldspace positions and distance + for (i=GetNumContacts()-1;i>=0;i--) + { + ManifoldPoint &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 + SimdScalar distance2d; + SimdVector3 projectedDifference,projectedPoint; + for (i=GetNumContacts()-1;i>=0;i--) + { + + ManifoldPoint &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/Bullet/NarrowPhaseCollision/PersistentManifold.h b/Bullet/NarrowPhaseCollision/PersistentManifold.h new file mode 100644 index 000000000..7e4852d31 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/PersistentManifold.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 "SimdVector3.h" +#include "SimdTransform.h" +#include "ManifoldPoint.h" + +struct CollisionResult; + +///contact breaking and merging treshold +extern float gContactBreakingTreshold; + +typedef bool (*ContactDestroyedCallback)(void* userPersistentData); + +extern ContactDestroyedCallback gContactCallback; + + + +#define MANIFOLD_CACHE_SIZE 4 + +///PersistentManifold maintains contact points, and reduces them to 4. +///It does contact filtering/contact reduction. +class PersistentManifold +{ + + ManifoldPoint 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 ManifoldPoint& pt); + + int FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt); + +public: + + int m_index1; + + PersistentManifold(); + + PersistentManifold(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(ManifoldPoint& pt); + +#ifdef DEBUG_PERSISTENCY + void DebugPersistency(); +#endif // + + inline int GetNumContacts() const { return m_cachedPoints;} + + inline const ManifoldPoint& GetContactPoint(int index) const + { + ASSERT(index < m_cachedPoints); + return m_pointCache[index]; + } + + inline ManifoldPoint& 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 ManifoldPoint& newPoint) const; + + void AddManifoldPoint( const ManifoldPoint& 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 ManifoldPoint& newPoint,int insertIndex) + { + assert(ValidContactDistance(newPoint)); + + ClearUserCache(m_pointCache[insertIndex]); + + m_pointCache[insertIndex] = newPoint; + } + + bool ValidContactDistance(const ManifoldPoint& pt) const + { + return pt.m_distance1 <= GetContactBreakingTreshold(); + } + /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin + void RefreshContactPoints( const SimdTransform& trA,const SimdTransform& trB); + + void ClearManifold(); + + + +}; + + + +#endif //PERSISTENT_MANIFOLD_H diff --git a/Bullet/NarrowPhaseCollision/PointCollector.h b/Bullet/NarrowPhaseCollision/PointCollector.h new file mode 100644 index 000000000..a8e632460 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/PointCollector.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 POINT_COLLECTOR_H +#define POINT_COLLECTOR_H + +#include "DiscreteCollisionDetectorInterface.h" + + + +struct PointCollector : public DiscreteCollisionDetectorInterface::Result +{ + + + SimdVector3 m_normalOnBInWorld; + SimdVector3 m_pointInWorld; + SimdScalar m_distance;//negative means penetration + + bool m_hasResult; + + PointCollector () + : m_distance(1e30f),m_hasResult(false) + { + } + + virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& 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/Bullet/NarrowPhaseCollision/RaycastCallback.cpp b/Bullet/NarrowPhaseCollision/RaycastCallback.cpp new file mode 100644 index 000000000..209d9612b --- /dev/null +++ b/Bullet/NarrowPhaseCollision/RaycastCallback.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 "RaycastCallback.h" + +TriangleRaycastCallback::TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to) + : + m_from(from), + m_to(to), + m_hitFraction(1.f) +{ + +} + + + +void TriangleRaycastCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) +{ + + + const SimdVector3 &vert0=triangle[0]; + const SimdVector3 &vert1=triangle[1]; + const SimdVector3 &vert2=triangle[2]; + + SimdVector3 v10; v10 = vert1 - vert0 ; + SimdVector3 v20; v20 = vert2 - vert0 ; + + SimdVector3 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; + SimdVector3 point; point.setInterpolate3( m_from, m_to, distance); + { + SimdVector3 v0p; v0p = vert0 - point; + SimdVector3 v1p; v1p = vert1 - point; + SimdVector3 cp0; cp0 = v0p.cross( v1p ); + + if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) + { + + + SimdVector3 v2p; v2p = vert2 - point; + SimdVector3 cp1; + cp1 = v1p.cross( v2p); + if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) + { + SimdVector3 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/Bullet/NarrowPhaseCollision/RaycastCallback.h b/Bullet/NarrowPhaseCollision/RaycastCallback.h new file mode 100644 index 000000000..463c576c5 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/RaycastCallback.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 "CollisionShapes/TriangleCallback.h" +struct BroadphaseProxy; + + +class TriangleRaycastCallback: public TriangleCallback +{ +public: + + //input + SimdVector3 m_from; + SimdVector3 m_to; + + float m_hitFraction; + + TriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to); + + virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex); + + virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex ) = 0; + +}; + +#endif //RAYCAST_TRI_CALLBACK_H + diff --git a/Bullet/NarrowPhaseCollision/Shape.cpp b/Bullet/NarrowPhaseCollision/Shape.cpp new file mode 100644 index 000000000..54cb6ad6d --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Shape.cpp @@ -0,0 +1,34 @@ +// Bullet Continuous Collision Detection and Physics Library +// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +// +// +// Shape.cpp +// +// 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. + + +#ifdef WIN32 +#if _MSC_VER >= 1310 + +#include "shape.h" + +Shape::Shape() +{ +} + +Shape::~Shape() +{ +} + +#endif +#endif //WIN32 diff --git a/Bullet/NarrowPhaseCollision/Shape.h b/Bullet/NarrowPhaseCollision/Shape.h new file mode 100644 index 000000000..a9200fe2c --- /dev/null +++ b/Bullet/NarrowPhaseCollision/Shape.h @@ -0,0 +1,67 @@ +// Bullet Continuous Collision Detection and Physics Library +// Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ +// +// +// Shape.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. +// +// Shape.h +// +#ifndef BULLET_SHAPE_H +#define BULLET_SHAPE_H + +#include "Maths.h" + + + + +struct Separation +{ + + short m_featureA; + short m_featureB; + float m_dist; + Vector3 m_axis; // in world space + + // separators + enum + { + kFeatureNone, // not separated + kFeatureA, + kFeatureB, + kFeatureBoth + }; + short m_separator; + + // contact between the 2 bodies (-1 if none) + short m_contact; +}; + +///Shape provides a interface for Hull class (convex hull calculation). +class Shape +{ +public: + Shape(); + virtual ~Shape(); + + //virtual void ComputeInertia(Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0; + virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const = 0; + + + virtual Bounds3 ComputeBounds(const Transform& transform) const = 0; +}; + + + +#endif //BULLET_SHAPE_H diff --git a/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h b/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h new file mode 100644 index 000000000..6a15d8e4f --- /dev/null +++ b/Bullet/NarrowPhaseCollision/SimplexSolverInterface.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 "SimdVector3.h" +#include "SimdPoint3.h" + +#define NO_VIRTUAL_INTERFACE 1 +#ifdef NO_VIRTUAL_INTERFACE +#include "VoronoiSimplexSolver.h" +#define SimplexSolverInterface VoronoiSimplexSolver +#else + +/// SimplexSolverInterface 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 SimplexSolverInterface +{ + public: + virtual ~SimplexSolverInterface() {}; + + virtual void reset() = 0; + + virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q) = 0; + + virtual bool closest(SimdVector3& v) = 0; + + virtual SimdScalar maxVertex() = 0; + + virtual bool fullSimplex() const = 0; + + virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const = 0; + + virtual bool inSimplex(const SimdVector3& w) = 0; + + virtual void backup_closest(SimdVector3& v) = 0; + + virtual bool emptySimplex() const = 0; + + virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) = 0; + + virtual int numVertices() const =0; + + +}; +#endif +#endif //SIMPLEX_SOLVER_INTERFACE_H + diff --git a/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp b/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp new file mode 100644 index 000000000..a91326198 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp @@ -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. +*/ + + +#include "SubSimplexConvexCast.h" +#include "CollisionShapes/ConvexShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" +#include "NarrowPhaseCollision/SimplexSolverInterface.h" + + +SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver) +:m_simplexSolver(simplexSolver), +m_convexA(convexA),m_convexB(convexB) +{ +} + + +#define MAX_ITERATIONS 1000 + +bool SubsimplexConvexCast::calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result) +{ + + MinkowskiSumShape combi(m_convexA,m_convexB); + MinkowskiSumShape* convex = &combi; + + SimdTransform rayFromLocalA; + SimdTransform rayToLocalA; + + rayFromLocalA = fromA.inverse()* fromB; + rayToLocalA = toA.inverse()* toB; + + + m_simplexSolver->reset(); + + convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis())); + + //float radius = 0.01f; + + SimdScalar lambda = 0.f; + //todo: need to verify this: + //because of minkowski difference, we need the inverse direction + + SimdVector3 s = -rayFromLocalA.getOrigin(); + SimdVector3 r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin()); + SimdVector3 x = s; + SimdVector3 v; + SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r); + + v = x - arbitraryPoint; + + int maxIter = MAX_ITERATIONS; + + SimdVector3 n; + n.setValue(0.f,0.f,0.f); + bool hasResult = false; + SimdVector3 c; + + float lastLambda = lambda; + + + float dist2 = v.length2(); + float epsilon = 0.0001f; + + SimdVector3 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/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h b/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h new file mode 100644 index 000000000..9fc58e133 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.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 "ConvexCast.h" +#include "SimplexSolverInterface.h" +class ConvexShape; + +/// SubsimplexConvexCast implements Gino van den Bergens' paper +///"Ray Casting against General 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 SubsimplexConvexCast : public ConvexCast +{ + SimplexSolverInterface* m_simplexSolver; + ConvexShape* m_convexA; + ConvexShape* m_convexB; + +public: + + SubsimplexConvexCast (ConvexShape* shapeA,ConvexShape* shapeB,SimplexSolverInterface* simplexSolver); + + //virtual ~SubsimplexConvexCast(); + ///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 GjkPairDetector. + virtual bool calcTimeOfImpact( + const SimdTransform& fromA, + const SimdTransform& toA, + const SimdTransform& fromB, + const SimdTransform& toB, + CastResult& result); + +}; + +#endif //SUBSIMPLEX_CONVEX_CAST_H diff --git a/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.cpp b/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.cpp new file mode 100644 index 000000000..f0b69cfe7 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.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 "VoronoiSimplexSolver.h" +#include +#include + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define CATCH_DEGENERATE_TETRAHEDRON 1 +void VoronoiSimplexSolver::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 VoronoiSimplexSolver::ReduceVertices (const UsageBitfield& 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 VoronoiSimplexSolver::reset() +{ + m_cachedValidClosest = false; + m_numVertices = 0; + m_needsUpdate = true; + m_lastW = SimdVector3(1e30f,1e30f,1e30f); + m_cachedBC.Reset(); +} + + + + //add a vertex +void VoronoiSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& 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 VoronoiSimplexSolver::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 SimdVector3& from = m_simplexVectorW[0]; + const SimdVector3& to = m_simplexVectorW[1]; + SimdVector3 nearest; + + SimdVector3 p (0.f,0.f,0.f); + SimdVector3 diff = p - from; + SimdVector3 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 + SimdVector3 p (0.f,0.f,0.f); + + const SimdVector3& a = m_simplexVectorW[0]; + const SimdVector3& b = m_simplexVectorW[1]; + const SimdVector3& 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: + { + + + SimdVector3 p (0.f,0.f,0.f); + + const SimdVector3& a = m_simplexVectorW[0]; + const SimdVector3& b = m_simplexVectorW[1]; + const SimdVector3& c = m_simplexVectorW[2]; + const SimdVector3& 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 VoronoiSimplexSolver::closest(SimdVector3& v) +{ + bool succes = UpdateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +SimdScalar VoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + SimdScalar maxV = 0.f; + for (i=0;i= 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 + SimdVector3 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 VoronoiSimplexSolver::PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d) +{ + SimdVector3 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 VoronoiSimplexSolver::ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult) +{ + SubSimplexClosestResult 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); + SimdPoint3 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); + SimdPoint3 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); + SimdPoint3 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); + SimdPoint3 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/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.h b/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.h new file mode 100644 index 000000000..defa071b9 --- /dev/null +++ b/Bullet/NarrowPhaseCollision/VoronoiSimplexSolver.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 VoronoiSimplexSolver_H +#define VoronoiSimplexSolver_H + +#include "SimplexSolverInterface.h" + + + +#define VORONOI_SIMPLEX_MAX_VERTS 5 + +struct UsageBitfield{ + UsageBitfield() + { + 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 SubSimplexClosestResult +{ + SimdPoint3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + UsageBitfield 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; + } + +}; + +/// VoronoiSimplexSolver 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 VoronoiSimplexSolver +#else +class VoronoiSimplexSolver : public SimplexSolverInterface +#endif +{ +public: + + int m_numVertices; + + SimdVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + SimdPoint3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + SimdPoint3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + + + + SimdPoint3 m_cachedP1; + SimdPoint3 m_cachedP2; + SimdVector3 m_cachedV; + SimdVector3 m_lastW; + bool m_cachedValidClosest; + + SubSimplexClosestResult m_cachedBC; + + bool m_needsUpdate; + + void removeVertex(int index); + void ReduceVertices (const UsageBitfield& usedVerts); + bool UpdateClosestVectorAndPoints(); + + bool ClosestPtPointTetrahedron(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d, SubSimplexClosestResult& finalResult); + int PointOutsideOfPlane(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c, const SimdPoint3& d); + bool ClosestPtPointTriangle(const SimdPoint3& p, const SimdPoint3& a, const SimdPoint3& b, const SimdPoint3& c,SubSimplexClosestResult& result); + +public: + + void reset(); + + void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q); + + + bool closest(SimdVector3& v); + + SimdScalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const; + + bool inSimplex(const SimdVector3& w); + + void backup_closest(SimdVector3& v) ; + + bool emptySimplex() const ; + + void compute_points(SimdPoint3& p1, SimdPoint3& p2) ; + + int numVertices() const + { + return m_numVertices; + } + + +}; + +#endif //VoronoiSimplexSolver diff --git a/BulletDocs.chi b/BulletDocs.chi new file mode 100644 index 000000000..e489c5472 Binary files /dev/null and b/BulletDocs.chi differ diff --git a/BulletDocs.chm b/BulletDocs.chm new file mode 100644 index 000000000..87c0514ce Binary files /dev/null and b/BulletDocs.chm differ diff --git a/BulletDynamics/ConstraintSolver/ConstraintSolver.h b/BulletDynamics/ConstraintSolver/ConstraintSolver.h new file mode 100644 index 000000000..a1975c187 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/ConstraintSolver.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 PersistentManifold; +class RigidBody; + +struct ContactSolverInfo; +struct BroadphaseProxy; +class IDebugDraw; + +/// ConstraintSolver provides solver interface +class ConstraintSolver +{ + +public: + + virtual ~ConstraintSolver() {} + + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0; + +}; + + + + +#endif //CONSTRAINT_SOLVER_H diff --git a/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/BulletDynamics/ConstraintSolver/ContactConstraint.cpp new file mode 100644 index 000000000..e55c18e47 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -0,0 +1,248 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "ContactConstraint.h" +#include "Dynamics/RigidBody.h" +#include "SimdVector3.h" +#include "JacobianEntry.h" +#include "ContactSolverInfo.h" +#include "GEN_MinMax.h" +#include "NarrowPhaseCollision/ManifoldPoint.h" + +#define ASSERT2 assert + +//some values to find stable tresholds + +float useGlobalSettingContacts = false;//true; +SimdScalar contactDamping = 0.2f; +SimdScalar contactTau = .02f;//0.02f;//*0.02f; + + + + +SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) +{ + SimdScalar friction = body0.getFriction() * body1.getFriction(); + + const SimdScalar MAX_FRICTION = 10.f; + if (friction < -MAX_FRICTION) + friction = -MAX_FRICTION; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + + +//bilateral constraint between two dynamic objects +void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, + RigidBody& body2, const SimdVector3& pos2, + SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep) +{ + float normalLenSqr = normal.length2(); + ASSERT2(fabs(normalLenSqr) < 1.1f); + if (normalLenSqr > 1.1f) + { + impulse = 0.f; + return; + } + SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + + JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), + body2.getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), + body2.getInvInertiaDiagLocal(),body2.getInvMass()); + + SimdScalar jacDiagAB = jac.getDiagonal(); + SimdScalar jacDiagABInv = 1.f / jacDiagAB; + + SimdScalar 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 + SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); + impulse = - contactDamping * rel_vel * massTerm; +#else + SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse = velocityImpulse; +#endif +} + + + + +//velocity + friction +//response between two dynamic objects with friction +float resolveSingleCollision( + RigidBody& body1, + RigidBody& body2, + ManifoldPoint& contactPoint, + const ContactSolverInfo& solverInfo + + ) +{ + + const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA(); + const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB(); + + +// printf("distance=%f\n",distance); + + const SimdVector3& normal = contactPoint.m_normalWorldOnB; + + SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + SimdScalar rel_vel; + rel_vel = normal.dot(vel); + + float combinedRestitution = body1.getRestitution() * body2.getRestitution(); + + + + SimdScalar 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); + + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance(); + + + //distance = 0.f; + SimdScalar positionalError = Kcor *-distance; + //jacDiagABInv; + SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping; + + + SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv; + + SimdScalar velocityImpulse = velocityError * cpd->m_jacDiagABInv; + + SimdScalar 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( + RigidBody& body1, + RigidBody& body2, + ManifoldPoint& contactPoint, + const ContactSolverInfo& solverInfo + + ) +{ + const SimdVector3& pos1 = contactPoint.GetPositionWorldOnA(); + const SimdVector3& pos2 = contactPoint.GetPositionWorldOnB(); + const SimdVector3& normal = contactPoint.m_normalWorldOnB; + + SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + float combinedFriction = calculateCombinedFriction(body1,body2); + + ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData; + assert(cpd); + + SimdScalar limit = cpd->m_appliedImpulse * combinedFriction; + //if (contactPoint.m_appliedImpulse>0.f) + //friction + { + //apply friction in the 2 tangential directions + + SimdScalar relaxation = solverInfo.m_damping; + { + // 1st tangent + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + SimdScalar vrel = cpd->m_frictionWorldTangential0.dot(vel); + + // calculate j that moves us to zero relative velocity + SimdScalar 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 + SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + SimdScalar vrel = cpd->m_frictionWorldTangential1.dot(vel); + + // calculate j that moves us to zero relative velocity + SimdScalar 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/BulletDynamics/ConstraintSolver/ContactConstraint.h b/BulletDynamics/ConstraintSolver/ContactConstraint.h new file mode 100644 index 000000000..847f2881f --- /dev/null +++ b/BulletDynamics/ConstraintSolver/ContactConstraint.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 CONTACT_CONSTRAINT_H +#define CONTACT_CONSTRAINT_H + +//todo: make into a proper class working with the iterative constraint solver + +class RigidBody; +#include "SimdVector3.h" +#include "SimdScalar.h" +struct ContactSolverInfo; +class ManifoldPoint; + +struct ConstraintPersistentData +{ + inline ConstraintPersistentData() + :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_penetration(0.f) + { + } + + + /// 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_penetration; + SimdVector3 m_frictionWorldTangential0; + SimdVector3 m_frictionWorldTangential1; + + +}; + +///bilateral constraint between two dynamic objects +///positive distance = separation, negative distance = penetration +void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, + RigidBody& body2, const SimdVector3& pos2, + SimdScalar distance, const SimdVector3& normal,SimdScalar& 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( + RigidBody& body1, + RigidBody& body2, + ManifoldPoint& contactPoint, + const ContactSolverInfo& info); + +float resolveSingleFriction( + RigidBody& body1, + RigidBody& body2, + ManifoldPoint& contactPoint, + const ContactSolverInfo& solverInfo + ); + +#endif //CONTACT_CONSTRAINT_H diff --git a/BulletDynamics/ConstraintSolver/ContactSolverInfo.h b/BulletDynamics/ConstraintSolver/ContactSolverInfo.h new file mode 100644 index 000000000..2ec9b7f6c --- /dev/null +++ b/BulletDynamics/ConstraintSolver/ContactSolverInfo.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 ContactSolverInfo +{ + + inline ContactSolverInfo() + { + 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/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/BulletDynamics/ConstraintSolver/HingeConstraint.cpp new file mode 100644 index 000000000..12e02f283 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -0,0 +1,176 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "HingeConstraint.h" +#include "Dynamics/RigidBody.h" +#include "Dynamics/MassProps.h" +#include "SimdTransformUtil.h" + + +HingeConstraint::HingeConstraint() +{ +} + +HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB, + SimdVector3& axisInA,SimdVector3& axisInB) +:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), +m_axisInA(axisInA), +m_axisInB(axisInB), +m_angularOnly(false) +{ + +} + + +HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA) +:TypedConstraint(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 HingeConstraint::BuildJacobian() +{ + SimdVector3 normal(0,0,0); + + if (!m_angularOnly) + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) JacobianEntry( + 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 ununsed for now, it's a todo + SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + SimdVector3 jointAxis0; + SimdVector3 jointAxis1; + SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); + + new (&m_jacAng[0]) JacobianEntry(jointAxis0, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + new (&m_jacAng[1]) JacobianEntry(jointAxis1, + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + m_rbA.getInvInertiaDiagLocal(), + m_rbB.getInvInertiaDiagLocal()); + + +} + +void HingeConstraint::SolveConstraint(SimdScalar timeStep) +{ + + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + SimdVector3 normal(0,0,0); + SimdScalar tau = 0.3f; + SimdScalar damping = 1.f; + + if (!m_angularOnly) + { + for (int i=0;i<3;i++) + { + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + SimdScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + + SimdVector3 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 + SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity(); + const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity(); + SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA); + SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB); + SimdVector3 velrel = angA-angB; + + //solve angular velocity correction + float relaxation = 1.f; + float len = velrel.length(); + if (len > 0.00001f) + { + SimdVector3 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 + SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + SimdVector3 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); + +} + +void HingeConstraint::UpdateRHS(SimdScalar timeStep) +{ + +} + diff --git a/BulletDynamics/ConstraintSolver/HingeConstraint.h b/BulletDynamics/ConstraintSolver/HingeConstraint.h new file mode 100644 index 000000000..a43463511 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/HingeConstraint.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 "SimdVector3.h" + +#include "ConstraintSolver/JacobianEntry.h" +#include "TypedConstraint.h" + +class RigidBody; + + +/// 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 HingeConstraint : public TypedConstraint +{ + JacobianEntry m_jac[3]; //3 orthogonal linear constraints + JacobianEntry m_jacAng[2]; //2 orthogonal angular constraints + + SimdVector3 m_pivotInA; + SimdVector3 m_pivotInB; + SimdVector3 m_axisInA; + SimdVector3 m_axisInB; + + bool m_angularOnly; + +public: + + HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,SimdVector3& axisInA,SimdVector3& axisInB); + + HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA); + + HingeConstraint(); + + virtual void BuildJacobian(); + + virtual void SolveConstraint(SimdScalar timeStep); + + void UpdateRHS(SimdScalar timeStep); + + const RigidBody& GetRigidBodyA() const + { + return m_rbA; + } + const RigidBody& GetRigidBodyB() const + { + return m_rbB; + } + + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + + + +}; + +#endif //HINGECONSTRAINT_H diff --git a/BulletDynamics/ConstraintSolver/JacobianEntry.h b/BulletDynamics/ConstraintSolver/JacobianEntry.h new file mode 100644 index 000000000..821c681a7 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/JacobianEntry.h @@ -0,0 +1,134 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef JACOBIAN_ENTRY_H +#define JACOBIAN_ENTRY_H + +#include "SimdVector3.h" +#include "Dynamics/RigidBody.h" + + +//notes: +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components +// which makes the JacobianEntry 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 JacobianEntry +{ +public: + JacobianEntry() {}; + //constraint between two different rigidbodies + JacobianEntry( + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + const SimdVector3& rel_pos1,const SimdVector3& rel_pos2, + const SimdVector3& jointAxis, + const SimdVector3& inertiaInvA, + const SimdScalar massInvA, + const SimdVector3& inertiaInvB, + const SimdScalar massInvB) + :m_jointAxis(jointAxis) + { + m_aJ = world2A*(rel_pos1.cross(m_jointAxis)); + m_bJ = world2B*(rel_pos2.cross(-m_jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ); + } + + //angular constraint between two different rigidbodies + JacobianEntry(const SimdVector3& jointAxis, + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + const SimdVector3& inertiaInvA, + const SimdVector3& inertiaInvB) + :m_jointAxis(m_jointAxis) + { + m_aJ= world2A*m_jointAxis; + m_bJ = world2B*-m_jointAxis; + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = inertiaInvB * m_bJ; + m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ); + } + + //constraint on one rigidbody + JacobianEntry( + const SimdMatrix3x3& world2A, + const SimdVector3& rel_pos1,const SimdVector3& rel_pos2, + const SimdVector3& jointAxis, + const SimdVector3& inertiaInvA, + const SimdScalar massInvA) + :m_jointAxis(jointAxis) + { + m_aJ= world2A*(rel_pos1.cross(m_jointAxis)); + m_bJ = world2A*(rel_pos2.cross(-m_jointAxis)); + m_0MinvJt = inertiaInvA * m_aJ; + m_1MinvJt = SimdVector3(0.f,0.f,0.f); + m_Adiag = massInvA + m_0MinvJt.dot(m_aJ); + } + + SimdScalar getDiagonal() const { return m_Adiag; } + + // for two constraints on the same rigidbody (for example vehicle friction) + SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const + { + const JacobianEntry& jacA = *this; + SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis); + SimdScalar 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) + SimdScalar getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const + { + const JacobianEntry& jacA = *this; + SimdVector3 lin = jacA.m_jointAxis* jacB.m_jointAxis; + SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ; + SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ; + SimdVector3 lin0 = massInvA * lin ; + SimdVector3 lin1 = massInvB * lin; + SimdVector3 sum = ang0+ang1+lin0+lin1; + return sum[0]+sum[1]+sum[2]; + } + + SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB) + { + SimdVector3 linrel = linvelA - linvelB; + SimdVector3 angvela = angvelA * m_aJ; + SimdVector3 angvelb = angvelB * m_bJ; + linrel *= m_jointAxis; + angvela += angvelb; + angvela += linrel; + SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2]; + return rel_vel2 + SIMD_EPSILON; + } +//private: + + SimdVector3 m_jointAxis; + SimdVector3 m_aJ; + SimdVector3 m_bJ; + SimdVector3 m_0MinvJt; + SimdVector3 m_1MinvJt; + //Optimization: can be stored in the w/last component of one of the vectors + SimdScalar m_Adiag; + +}; + +#endif //JACOBIAN_ENTRY_H diff --git a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp new file mode 100644 index 000000000..8febbe2ec --- /dev/null +++ b/BulletDynamics/ConstraintSolver/OdeConstraintSolver.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 "OdeConstraintSolver.h" + +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "Dynamics/RigidBody.h" +#include "ContactConstraint.h" +#include "Solve2LinearConstraint.h" +#include "ContactSolverInfo.h" +#include "Dynamics/BU_Joint.h" +#include "Dynamics/ContactJoint.h" + +#include "IDebugDraw.h" + +#define USE_SOR_SOLVER + +#include "SorLcp.h" + +#include +#include //FLT_MAX +#ifdef WIN32 +#include +#endif +#include +#include + +#if defined (WIN32) +#include +#else +#if defined (__FreeBSD__) +#include +#else +#include +#endif +#endif + +class BU_Joint; + +//see below + +OdeConstraintSolver::OdeConstraintSolver(): +m_cfm(0.f),//1e-5f), +m_erp(0.4f) +{ +} + + + + + + + +//iterative lcp and penalty method +float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) +{ + m_CurBody = 0; + m_CurJoint = 0; + + + RigidBody* bodies [MAX_RIGIDBODIES]; + + int numBodies = 0; + BU_Joint* joints [MAX_RIGIDBODIES*4]; + int numJoints = 0; + + for (int j=0;jGetNumContacts() > 0) + { + body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); + body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); + ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); + } + } + + SolveInternal1(m_cfm,m_erp,bodies,numBodies,joints,numJoints,infoGlobal); + + return 0.f; + +} + +///////////////////////////////////////////////////////////////////////////////// + + +typedef SimdScalar dQuaternion[4]; +#define _R(i,j) R[(i)*4+(j)] + +void dRfromQ1 (dMatrix3 R, const dQuaternion q) +{ + // q = (s,vx,vy,vz) + SimdScalar qq1 = 2.f*q[1]*q[1]; + SimdScalar qq2 = 2.f*q[2]*q[2]; + SimdScalar qq3 = 2.f*q[3]*q[3]; + _R(0,0) = 1.f - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = 0.f; + + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1.f - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = 0.f; + + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1.f - qq1 - qq2; + _R(2,3) = 0.f; + +} + + + +int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) +{ + if (!body || (body->getInvMass() == 0.f) ) + { + return -1; + } + //first try to find + int i,j; + for (i=0;im_facc.setValue(0,0,0,0); + body->m_tacc.setValue(0,0,0,0); + + //are the indices the same ? + for (i=0;i<4;i++) + { + for ( j=0;j<3;j++) + { + body->m_invI[i+4*j] = 0.f; + body->m_I[i+4*j] = 0.f; + } + } + body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0]; + body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; + body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; + + body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0]; + body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1]; + body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2]; + + + + + dQuaternion q; + + q[1] = body->getOrientation()[0]; + q[2] = body->getOrientation()[1]; + q[3] = body->getOrientation()[2]; + q[0] = body->getOrientation()[3]; + + dRfromQ1(body->m_R,q); + + return numBodies-1; +} + + + + + +#define MAX_JOINTS_1 8192 + +static ContactJoint gJointArray[MAX_JOINTS_1]; + + +void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) +{ + + + manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), + ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + + int i,numContacts = manifold->GetNumContacts(); + + bool swapBodies = (bodyId0 < 0); + + + RigidBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = (RigidBody*)manifold->GetBody1(); + body1 = (RigidBody*)manifold->GetBody0(); + + } else + { + body0 = (RigidBody*)manifold->GetBody0(); + body1 = (RigidBody*)manifold->GetBody1(); + } + + assert(bodyId0 >= 0); + + SimdVector3 color(0,1,0); + for (i=0;iGetContactPoint(i); + + debugDrawer->DrawContactPoint( + cp.m_positionWorldOnB, + cp.m_normalWorldOnB, + cp.GetDistance(), + cp.GetLifeTime(), + color); + + } + assert (m_CurJoint < MAX_JOINTS_1); + +// if (manifold->GetContactPoint(i).GetDistance() < 0.0f) + { + ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); + + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + joints[numJoints++] = cont; + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + } + + //create a new contact constraint +}; + diff --git a/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h new file mode 100644 index 000000000..99840557a --- /dev/null +++ b/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h @@ -0,0 +1,66 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef ODE_CONSTRAINT_SOLVER_H +#define ODE_CONSTRAINT_SOLVER_H + +#include "ConstraintSolver.h" + +class RigidBody; +class BU_Joint; + +/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework +/// It uses the the unmodified version of quickstep solver from the open dynamics project +class OdeConstraintSolver : public ConstraintSolver +{ +private: + + int m_CurBody; + int m_CurJoint; + + float m_cfm; + float m_erp; + + + int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); + void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); + +public: + + OdeConstraintSolver(); + + virtual ~OdeConstraintSolver() {} + + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); + + ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal + ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' + void setConstraintForceMixing(float cfm) { + m_cfm = cfm; + } + + ///setErrorReductionParamter sets the maximum amount of error reduction + ///which limits energy addition during penetration depth recovery + void setErrorReductionParamter(float erp) + { + m_erp = erp; + } +}; + + + + +#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp new file mode 100644 index 000000000..64b470f43 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp @@ -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. +*/ + + +#include "Point2PointConstraint.h" +#include "Dynamics/RigidBody.h" +#include "Dynamics/MassProps.h" + + + + +Point2PointConstraint::Point2PointConstraint() +{ +} + +Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB) +:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +{ + +} + + +Point2PointConstraint::Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA) +:TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +{ + +} + +void Point2PointConstraint::BuildJacobian() +{ + SimdVector3 normal(0,0,0); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + new (&m_jac[i]) JacobianEntry( + 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 Point2PointConstraint::SolveConstraint(SimdScalar timeStep) +{ + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; + SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + + + SimdVector3 normal(0,0,0); + + +// SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); +// SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i=0;i<3;i++) + { + normal[i] = 1; + SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal(); + + SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + //this jacobian entry could be re-used for all iterations + + SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + + SimdScalar rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + //positional error (zeroth order error) + SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + + SimdScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; + + SimdVector3 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 Point2PointConstraint::UpdateRHS(SimdScalar timeStep) +{ + +} + diff --git a/BulletDynamics/ConstraintSolver/Point2PointConstraint.h b/BulletDynamics/ConstraintSolver/Point2PointConstraint.h new file mode 100644 index 000000000..bd28ac7b1 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/Point2PointConstraint.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 "SimdVector3.h" + +#include "ConstraintSolver/JacobianEntry.h" +#include "TypedConstraint.h" + +class RigidBody; + +struct ConstraintSetting +{ + ConstraintSetting() : + 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 Point2PointConstraint : public TypedConstraint +{ + JacobianEntry m_jac[3]; //3 orthogonal linear constraints + + SimdVector3 m_pivotInA; + SimdVector3 m_pivotInB; + + + +public: + + ConstraintSetting m_setting; + + Point2PointConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB); + + Point2PointConstraint(RigidBody& rbA,const SimdVector3& pivotInA); + + Point2PointConstraint(); + + virtual void BuildJacobian(); + + + virtual void SolveConstraint(SimdScalar timeStep); + + void UpdateRHS(SimdScalar timeStep); + + void SetPivotA(const SimdVector3& pivotA) + { + m_pivotInA = pivotA; + } + + void SetPivotB(const SimdVector3& pivotB) + { + m_pivotInB = pivotB; + } + + + +}; + +#endif //POINT2POINTCONSTRAINT_H diff --git a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp new file mode 100644 index 000000000..fe59a4068 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -0,0 +1,329 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "SimpleConstraintSolver.h" +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "Dynamics/RigidBody.h" +#include "ContactConstraint.h" +#include "Solve2LinearConstraint.h" +#include "ContactSolverInfo.h" +#include "Dynamics/BU_Joint.h" +#include "Dynamics/ContactJoint.h" +#include "IDebugDraw.h" +#include "JacobianEntry.h" +#include "GEN_MinMax.h" + +#ifdef USE_PROFILE +#include "quickprof.h" +#endif //USE_PROFILE + +int totalCpd = 0; + + + +bool MyContactDestroyedCallback(void* userPersistentData) +{ + assert (userPersistentData); + ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData; + delete cpd; + totalCpd--; + //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); + return true; +} + + +SimpleConstraintSolver::SimpleConstraintSolver() +{ + gContactCallback = &MyContactDestroyedCallback; +} + + +/// SimpleConstraintSolver Sequentially applies impulses +float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) +{ + + ContactSolverInfo info = infoGlobal; + + int numiter = infoGlobal.m_numIterations; +#ifdef USE_PROFILE + Profiler::beginBlock("Solve"); +#endif //USE_PROFILE + + //should traverse the contacts random order... + int i; + for ( i = 0;iGetBody0(); + RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); + + float maxImpulse = 0.f; + + //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop + if (iter == 0) + { + manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); + + int numpoints = manifoldPtr->GetNumContacts(); + + SimdVector3 color(0,1,0); + for (int i=0;iGetContactPoint(i); + if (cp.GetDistance() <= 0.f) + { + const SimdVector3& pos1 = cp.GetPositionWorldOnA(); + const SimdVector3& pos2 = cp.GetPositionWorldOnB(); + + SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); + SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition(); + + + //this jacobian entry is re-used for all iterations + JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(), + body1->getCenterOfMassTransform().getBasis().transpose(), + rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(), + body1->getInvInertiaDiagLocal(),body1->getInvMass()); + + + SimdScalar jacDiagAB = jac.getDiagonal(); + + ConstraintPersistentData* cpd = (ConstraintPersistentData*) 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) ConstraintPersistentData; + cpd->m_persistentLifeTime = cp.GetLifeTime(); + + } else + { + //printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime()); + + } + } else + { + + cpd = new ConstraintPersistentData(); + 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; + + + SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); + SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); + SimdVector3 vel = vel1 - vel2; + SimdScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + float combinedRestitution = body0->getRestitution() * body1->getRestitution(); + + cpd->m_penetration = cp.GetDistance(); + + 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 + + SimdScalar 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 + SimdPlaneSpace1(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; + + SimdVector3 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); + } + + } + } + + { + const int numpoints = manifoldPtr->GetNumContacts(); + + SimdVector3 color(0,1,0); + for (int i=0;iGetContactPoint(j); + if (cp.GetDistance() <= 0.f) + { + + if (iter == 0) + { + if (debugDrawer) + debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color); + } + + { + + + //float dist = cp.GetDistance(); + //printf("dist(%i)=%f\n",j,dist); + float impulse = resolveSingleCollision( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + +float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +{ + RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); + RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); + + + { + const int numpoints = manifoldPtr->GetNumContacts(); + + SimdVector3 color(0,1,0); + for (int i=0;iGetContactPoint(j); + if (cp.GetDistance() <= 0.f) + { + + resolveSingleFriction( + *body0,*body1, + cp, + info); + + } + } + + + } + return 0.f; +} diff --git a/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h new file mode 100644 index 000000000..99b77623b --- /dev/null +++ b/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h @@ -0,0 +1,43 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_CONSTRAINT_SOLVER_H +#define SIMPLE_CONSTRAINT_SOLVER_H + +#include "ConstraintSolver.h" +class IDebugDraw; + +/// SimpleConstraintSolver 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 SimpleConstraintSolver : public ConstraintSolver +{ + float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); + float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); + + +public: + + SimpleConstraintSolver(); + + virtual ~SimpleConstraintSolver() {} + + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); + +}; + +#endif //SIMPLE_CONSTRAINT_SOLVER_H + diff --git a/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.cpp b/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.cpp new file mode 100644 index 000000000..34bf3e488 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.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 "Solve2LinearConstraint.h" + +#include "Dynamics/RigidBody.h" +#include "SimdVector3.h" +#include "JacobianEntry.h" + + +void Solve2LinearConstraint::resolveUnilateralPairConstraint( + RigidBody* body1, + RigidBody* body2, + + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + + const SimdVector3& invInertiaADiag, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdVector3& invInertiaBDiag, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + SimdScalar 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 + JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + +// SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv + SimdScalar massTerm = 1.f / (invMassA + invMassB); + + + // calculate rhs (or error) terms + const SimdScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; + const SimdScalar 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!) + // + + + SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + SimdScalar 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 Solve2LinearConstraint::resolveBilateralPairConstraint( + RigidBody* body1, + RigidBody* body2, + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + + const SimdVector3& invInertiaADiag, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdVector3& invInertiaBDiag, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1) +{ + + imp0 = 0.f; + imp1 = 0.f; + + SimdScalar 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 + JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, + invInertiaBDiag,invMassB); + + //const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + //const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); + + const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1)); + const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1)); + + // calculate rhs (or error) terms + const SimdScalar dv0 = depthA * m_tau - vel0 * m_damping; + const SimdScalar dv1 = depthB * m_tau - vel1 * m_damping; + + // dC/dv * dv = -C + + // jacobian * impulse = -error + // + + //impulse = jacobianInverse * -error + + // inverting 2x2 symmetric system (offdiagonal are equal!) + // + + + SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); + SimdScalar 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 Solve2LinearConstraint::resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdMatrix3x3& invInertiaBWS, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1) +{ + +} + diff --git a/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.h b/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.h new file mode 100644 index 000000000..4a0986abf --- /dev/null +++ b/BulletDynamics/ConstraintSolver/Solve2LinearConstraint.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 "SimdMatrix3x3.h" +#include "SimdVector3.h" + + +class RigidBody; + + + +/// constraint class used for lateral tyre friction. +class Solve2LinearConstraint +{ + SimdScalar m_tau; + SimdScalar m_damping; + +public: + + Solve2LinearConstraint(SimdScalar tau,SimdScalar damping) + { + m_tau = tau; + m_damping = damping; + } + // + // solve unilateral constraint (equality, direct method) + // + void resolveUnilateralPairConstraint( + RigidBody* body0, + RigidBody* body1, + + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + + const SimdVector3& invInertiaADiag, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdVector3& invInertiaBDiag, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1); + + + // + // solving 2x2 lcp problem (inequality, direct solution ) + // + void resolveBilateralPairConstraint( + RigidBody* body0, + RigidBody* body1, + const SimdMatrix3x3& world2A, + const SimdMatrix3x3& world2B, + + const SimdVector3& invInertiaADiag, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdVector3& invInertiaBDiag, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1); + + + void resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS, + const SimdScalar invMassA, + const SimdVector3& linvelA,const SimdVector3& angvelA, + const SimdVector3& rel_posA1, + const SimdMatrix3x3& invInertiaBWS, + const SimdScalar invMassB, + const SimdVector3& linvelB,const SimdVector3& angvelB, + const SimdVector3& rel_posA2, + + SimdScalar depthA, const SimdVector3& normalA, + const SimdVector3& rel_posB1,const SimdVector3& rel_posB2, + SimdScalar depthB, const SimdVector3& normalB, + SimdScalar& imp0,SimdScalar& imp1); + + +}; + +#endif //SOLVE_2LINEAR_CONSTRAINT_H diff --git a/BulletDynamics/ConstraintSolver/SorLcp.cpp b/BulletDynamics/ConstraintSolver/SorLcp.cpp new file mode 100644 index 000000000..44ed3f057 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/SorLcp.cpp @@ -0,0 +1,849 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General 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 * + * General Public License is included with this library in the * + * file LICENSE.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.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#include "SorLcp.h" + +#ifdef USE_SOR_SOLVER + +// SOR LCP taken from ode quickstep, +// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver + + +#include "SimdScalar.h" + +#include "Dynamics/RigidBody.h" +#include +#include //FLT_MAX +#ifdef WIN32 +#include +#endif +#include +#include + +#if defined (WIN32) +#include +#else +#if defined (__FreeBSD__) +#include +#else +#include +#endif +#endif + +#include "Dynamics/BU_Joint.h" +#include "ContactSolverInfo.h" + +//////////////////////////////////////////////////////////////////// +//math stuff + +typedef SimdScalar dVector4[4]; +typedef SimdScalar dMatrix3[4*3]; +#define dInfinity FLT_MAX + + + +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ + + + +#define dMULTIPLY0_331NEW(A,op,B,C) \ +{\ + float tmp[3];\ + tmp[0] = C.getX();\ + tmp[1] = C.getY();\ + tmp[2] = C.getZ();\ + dMULTIPLYOP0_331(A,op,B,tmp);\ +} + +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLYOP0_331(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B+4),(C)); \ + (A)[2] op dDOT1((B+8),(C)); + +#define dAASSERT ASSERT +#define dIASSERT ASSERT + +#define REAL float +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); } +#define dDOT14(a,b) dDOTpq(a,b,1,4) + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + + +#define dMULTIPLYOP2_333(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B),(C+4)); \ + (A)[2] op dDOT1((B),(C+8)); \ + (A)[4] op dDOT1((B+4),(C)); \ + (A)[5] op dDOT1((B+4),(C+4)); \ + (A)[6] op dDOT1((B+4),(C+8)); \ + (A)[8] op dDOT1((B+8),(C)); \ + (A)[9] op dDOT1((B+8),(C+4)); \ + (A)[10] op dDOT1((B+8),(C+8)); +#define dMULTIPLYOP0_333(A,op,B,C) \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ + (A)[4] op dDOT14((B+4),(C)); \ + (A)[5] op dDOT14((B+4),(C+1)); \ + (A)[6] op dDOT14((B+4),(C+2)); \ + (A)[8] op dDOT14((B+8),(C)); \ + (A)[9] op dDOT14((B+8),(C+1)); \ + (A)[10] op dDOT14((B+8),(C+2)); + +#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) +#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) +#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) + + +//////////////////////////////////////////////////////////////////// +#define EFFICIENT_ALIGNMENT 16 +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + + + +///////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////// + +#ifdef DEBUG +#define ANSI_FTOL 1 + +extern "C" { + __declspec(naked) void _ftol2() { + __asm { +#if ANSI_FTOL + fnstcw WORD PTR [esp-2] + mov ax, WORD PTR [esp-2] + + OR AX, 0C00h + + mov WORD PTR [esp-4], ax + fldcw WORD PTR [esp-4] + fistp QWORD PTR [esp-12] + fldcw WORD PTR [esp-2] + mov eax, DWORD PTR [esp-12] + mov edx, DWORD PTR [esp-8] +#else + fistp DWORD PTR [esp-12] + mov eax, DWORD PTR [esp-12] + mov ecx, DWORD PTR [esp-8] +#endif + ret + } + } +} +#endif //DEBUG + + + + + + +#define ALLOCA dALLOCA16 + +typedef const SimdScalar *dRealPtr; +typedef SimdScalar *dRealMutablePtr; +#define dRealArray(name,n) SimdScalar name[n]; +#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar)); + +void dSetZero1 (SimdScalar *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + +void dSetValue1 (SimdScalar *a, int n, SimdScalar value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} + + +//*************************************************************************** +// configuration + +// for the SOR and CG methods: +// uncomment the following line to use warm starting. this definitely +// help for motor-driven joints. unfortunately it appears to hurt +// with high-friction contacts using the SOR method. use with care + +//#define WARM_STARTING 1 + +// for the SOR method: +// uncomment the following line to randomly reorder constraint rows +// during the solution. depending on the situation, this can help a lot +// or hardly at all, but it doesn't seem to hurt. + +#define RANDOMLY_REORDER_CONSTRAINTS 1 + + + +//*************************************************************************** +// various common computations involving the matrix J + +// compute iMJ = inv(M)*J' + +static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, + RigidBody * const *body, dRealPtr invI) +{ + int i,j; + dRealMutablePtr iMJ_ptr = iMJ; + dRealMutablePtr J_ptr = J; + for (i=0; igetInvMass(); + for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; + dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); + if (b2 >= 0) { + k = body[b2]->getInvMass(); + for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; + dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); + } + J_ptr += 12; + iMJ_ptr += 12; + } +} + +#if 0 +static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) +{ + int i,j; + + + + dRealMutablePtr out_ptr1 = out + onlyBody1*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + + if (onlyBody2 >= 0) + { + out_ptr1 = out + onlyBody2*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + } + + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) + { + out_ptr = out + b2*6; + for (j=0; j<6; j++) + out_ptr[j] += iMJ_ptr[j] * in[i]; + } + } + + iMJ_ptr += 6; + + } +} +#endif + + +// compute out = inv(M)*J'*in. + +#if 0 +static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dSetZero1 (out,6*nb); + dRealPtr iMJ_ptr = iMJ; + for (i=0; i= 0) { + out_ptr = out + b2*6; + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; + } + iMJ_ptr += 6; + } +} +#endif + + +// compute out = J*in. + +static void multiply_J (int m, dRealMutablePtr J, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dRealPtr J_ptr = J; + for (i=0; i= 0) { + in_ptr = in + b2*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + } + J_ptr += 6; + out[i] = sum; + } +} + +//*************************************************************************** +// SOR-LCP method + +// nb is the number of bodies in the body array. +// J is an m*12 matrix of constraint rows +// jb is an array of first and second body numbers for each constraint row +// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) +// +// this returns lambda and fc (the constraint force). +// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda +// +// b, lo and hi are modified on exit + + +struct IndexError { + SimdScalar error; // error to sort on + int findex; + int index; // row index +}; + +static unsigned long seed2 = 0; + +unsigned long dRand2() +{ + seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff; + return seed2; +} + +int dRandInt2 (int n) +{ + float a = float(n) / 4294967296.0f; + return (int) (float(dRand2()) * a); +} + + +static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + int numiter,float overRelax) +{ + const int num_iterations = numiter; + const float sor_w = overRelax; // SOR over-relaxation parameter + + int i,j; + +#ifdef WARM_STARTING + // for warm starting, this seems to be necessary to prevent + // jerkiness in motor-driven joints. i have no idea why this works. + for (i=0; i= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = sor_w / sum;//(sum + cfm[i]); + } + + // scale J and b by Ad + J_ptr = J; + for (i=0; i= 0) + order[j++].index = i; + dIASSERT (j==m); +#endif + + for (int iteration=0; iteration < num_iterations; iteration++) { + +#ifdef REORDER_CONSTRAINTS + // constraints with findex < 0 always come first. + if (iteration < 2) { + // for the first two iterations, solve the constraints in + // the given order + for (i=0; i v2) ? v1 : v2; + if (max > 0) { + //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; + order[i].error = dFabs(lambda[i]-last_lambda[i]); + } + else { + order[i].error = dInfinity; + } + order[i].findex = findex[i]; + order[i].index = i; + } + } + qsort (order,m,sizeof(IndexError),&compare_index_error); +#endif +#ifdef RANDOMLY_REORDER_CONSTRAINTS + if ((iteration & 7) == 0) { + for (i=1; i= 0) { + hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]); + lo[index] = -hi[index]; + } + + int b1 = jb[index*2]; + int b2 = jb[index*2+1]; + float delta = rhs[index] - lambda[index]*Ad[index]; + dRealMutablePtr fc_ptr = invMforce + 6*b1; + + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case + delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; + } + + // compute lambda and clamp it to [lo,hi]. + // @@@ potential optimization: does SSE have clamping instructions + // to save test+jump penalties here? + float new_lambda = lambda[index] + delta; + if (new_lambda < lo[index]) { + delta = lo[index]-lambda[index]; + lambda[index] = lo[index]; + } + else if (new_lambda > hi[index]) { + delta = hi[index]-lambda[index]; + lambda[index] = hi[index]; + } + else { + lambda[index] = new_lambda; + } + + //@@@ a trick that may or may not help + //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); + //delta *= ramp; + + // update invMforce. + // @@@ potential optimization: SIMD for this and the b2 >= 0 case + fc_ptr = invMforce + 6*b1; + fc_ptr[0] += delta * iMJ_ptr[0]; + fc_ptr[1] += delta * iMJ_ptr[1]; + fc_ptr[2] += delta * iMJ_ptr[2]; + fc_ptr[3] += delta * iMJ_ptr[3]; + fc_ptr[4] += delta * iMJ_ptr[4]; + fc_ptr[5] += delta * iMJ_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + fc_ptr[0] += delta * iMJ_ptr[6]; + fc_ptr[1] += delta * iMJ_ptr[7]; + fc_ptr[2] += delta * iMJ_ptr[8]; + fc_ptr[3] += delta * iMJ_ptr[9]; + fc_ptr[4] += delta * iMJ_ptr[10]; + fc_ptr[5] += delta * iMJ_ptr[11]; + } + } + } +} + + + +void SolveInternal1 (float global_cfm, + float global_erp, + RigidBody * const *body, int nb, + BU_Joint * const *_joint, + int nj, + const ContactSolverInfo& solverInfo) +{ + + int numIter = solverInfo.m_numIterations; + float sOr = solverInfo.m_sor; + + int i,j; + + SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep); + + // number all bodies in the body list - set their tag values + for (i=0; im_odeTag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "BU_Joint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints + BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); + memcpy (joint,_joint,nj * sizeof(BU_Joint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. + dRealAllocaArray (I,3*4*nb); + dRealAllocaArray (invI,3*4*nb); +/* for (i=0; im_I,body[i]->m_R); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force + dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); + } +*/ + for (i=0; im_I,body[i]->m_R); + dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); + + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force + dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity()); + dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); + } + + + + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + //@@@ do we really need to save all the info1's + BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1)); + for (i=0, j=0; jGetInfo1 (info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // create the row offset array + int m = 0; + int *ofs = (int*) alloca (nj*sizeof(int)); + for (i=0; i 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dRealAllocaArray (c,m); + dRealAllocaArray (cfm,m); + dRealAllocaArray (lo,m); + dRealAllocaArray (hi,m); + int *findex = (int*) alloca (m*sizeof(int)); + dSetZero1 (c,m); + dSetValue1 (cfm,m,global_cfm); + dSetValue1 (lo,m,-dInfinity); + dSetValue1 (hi,m, dInfinity); + for (i=0; iGetInfo2 (&Jinfo); + + if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) + Jinfo.c[0] = solverInfo.m_maxErrorReduction; + + + + + // adjust returned findex values for global index numbering + for (j=0; j= 0) + findex[ofs[i] + j] += ofs[i]; + } + } + + // create an array of body numbers for each joint row + int *jb_ptr = jb; + for (i=0; inode[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; + int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; + for (j=0; jgetInvMass(); + for (j=0; j<3; j++) + tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; + dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); + for (j=0; j<3; j++) + tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; + } + + // put J*tmp1 into rhs + dRealAllocaArray (rhs,m); + multiply_J (m,J,jb,tmp1,rhs); + + // complete rhs + for (i=0; ilambda,info[i].m * sizeof(SimdScalar)); + } +#endif + + // solve the LCP problem and get lambda and invM*constraint_force + dRealAllocaArray (cforce,nb*6); + + SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr); + +#ifdef WARM_STARTING + // save lambda for the next iteration + //@@@ note that this doesn't work for contact joints yet, as they are + // recreated every iteration + for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar)); + } +#endif + + + // note that the SOR method overwrites rhs and J at this point, so + // they should not be used again. + + // add stepsize * cforce to the body velocity + for (i=0; igetLinearVelocity(); + SimdVector3 angvel = body[i]->getAngularVelocity(); + + for (j=0; j<3; j++) + linvel[j] += solverInfo.m_timeStep* cforce[i*6+j]; + for (j=0; j<3; j++) + angvel[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; + + body[i]->setLinearVelocity(linvel); + body[i]->setAngularVelocity(angvel); + + } + + } + + + + // compute the velocity update: + // add stepsize * invM * fe to the body velocity + + for (i=0; igetInvMass(); + SimdVector3 linvel = body[i]->getLinearVelocity(); + SimdVector3 angvel = body[i]->getAngularVelocity(); + + for (j=0; j<3; j++) + { + linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; + } + for (j=0; j<3; j++) + { + body[i]->m_tacc[j] *= solverInfo.m_timeStep; + } + dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); + body[i]->setAngularVelocity(angvel); + + } + + + +} + + +#endif //USE_SOR_SOLVER diff --git a/BulletDynamics/ConstraintSolver/SorLcp.h b/BulletDynamics/ConstraintSolver/SorLcp.h new file mode 100644 index 000000000..37fe09c26 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/SorLcp.h @@ -0,0 +1,45 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General 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 * + * General Public License is included with this library in the * + * file LICENSE.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.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + +#define USE_SOR_SOLVER +#ifdef USE_SOR_SOLVER + +#ifndef SOR_LCP_H +#define SOR_LCP_H +class RigidBody; +class BU_Joint; +#include "SimdScalar.h" + +struct ContactSolverInfo; + +void SolveInternal1 (float global_cfm, + float global_erp, + RigidBody * const *body, int nb, + BU_Joint * const *_joint, int nj, const ContactSolverInfo& info); + +int dRandInt2 (int n); + + +#endif //SOR_LCP_H + +#endif //USE_SOR_SOLVER + diff --git a/BulletDynamics/ConstraintSolver/TypedConstraint.cpp b/BulletDynamics/ConstraintSolver/TypedConstraint.cpp new file mode 100644 index 000000000..d1605b7a0 --- /dev/null +++ b/BulletDynamics/ConstraintSolver/TypedConstraint.cpp @@ -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. +*/ + + +#include "TypedConstraint.h" +#include "Dynamics/RigidBody.h" +#include "Dynamics/MassProps.h" + +static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f); + +TypedConstraint::TypedConstraint() +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(s_fixed), +m_rbB(s_fixed) +{ + s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); +} +TypedConstraint::TypedConstraint(RigidBody& rbA) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(s_fixed) +{ + s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); + +} + + +TypedConstraint::TypedConstraint(RigidBody& rbA,RigidBody& rbB) +: m_userConstraintType(-1), +m_userConstraintId(-1), +m_rbA(rbA), +m_rbB(rbB) +{ + s_fixed.setMassProps(0.f,SimdVector3(0.f,0.f,0.f)); + +} + diff --git a/BulletDynamics/ConstraintSolver/TypedConstraint.h b/BulletDynamics/ConstraintSolver/TypedConstraint.h new file mode 100644 index 000000000..50a5ab20f --- /dev/null +++ b/BulletDynamics/ConstraintSolver/TypedConstraint.h @@ -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. +*/ + +#ifndef TYPED_CONSTRAINT_H +#define TYPED_CONSTRAINT_H + +class RigidBody; +#include "SimdScalar.h" + +//TypedConstraint is the baseclass for Bullet constraints and vehicles +class TypedConstraint +{ + int m_userConstraintType; + int m_userConstraintId; + +protected: + RigidBody& m_rbA; + RigidBody& m_rbB; + +public: + + TypedConstraint(); + virtual ~TypedConstraint() {}; + TypedConstraint(RigidBody& rbA); + + TypedConstraint(RigidBody& rbA,RigidBody& rbB); + + virtual void BuildJacobian() = 0; + + virtual void SolveConstraint(SimdScalar timeStep) = 0; + + const RigidBody& GetRigidBodyA() const + { + return m_rbA; + } + const RigidBody& GetRigidBodyB() const + { + 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; + } +}; + +#endif //TYPED_CONSTRAINT_H diff --git a/BulletDynamics/Dynamics/BU_Joint.cpp b/BulletDynamics/Dynamics/BU_Joint.cpp new file mode 100644 index 000000000..ab2a75570 --- /dev/null +++ b/BulletDynamics/Dynamics/BU_Joint.cpp @@ -0,0 +1,25 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "BU_Joint.h" + +BU_Joint::BU_Joint() +{ + +} +BU_Joint::~BU_Joint() +{ + +} diff --git a/BulletDynamics/Dynamics/BU_Joint.h b/BulletDynamics/Dynamics/BU_Joint.h new file mode 100644 index 000000000..5c790bd02 --- /dev/null +++ b/BulletDynamics/Dynamics/BU_Joint.h @@ -0,0 +1,93 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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_Joint_H +#define BU_Joint_H + +class RigidBody; +class BU_Joint; +#include "SimdScalar.h" + +struct BU_ContactJointNode { + BU_Joint *joint; // pointer to enclosing BU_Joint object + RigidBody*body; // *other* body this joint is connected to +}; +typedef SimdScalar dVector3[4]; + + +class BU_Joint { + +public: + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + BU_Joint(); + virtual ~BU_Joint(); + + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // integrator parameters: frames per second (1/stepsize), default error + // reduction parameter (0..1). + SimdScalar fps,erp; + + // for the first and second body, pointers to two (linear and angular) + // n*3 jacobian sub matrices, stored by rows. these matrices will have + // been initialized to 0 on entry. if the second body is zero then the + // J2xx pointers may be 0. + SimdScalar *J1l,*J1a,*J2l,*J2a; + + // elements to jump from one row to the next in J's + int rowskip; + + // right hand sides of the equation J*v = c + cfm * lambda. cfm is the + // "constraint force mixing" vector. c is set to zero on entry, cfm is + // set to a constant value (typically very small or zero) value on entry. + SimdScalar *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + SimdScalar *lo,*hi; + + // findex vector for variables. see the LCP solver interface for a + // description of what this does. this is set to -1 on entry. + // note that the returned indexes are relative to the first index of + // the constraint. + int *findex; + }; + + // virtual function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + virtual void GetInfo1 (Info1 *info)=0; + virtual void GetInfo2 (Info2 *info)=0; + + int flags; // dJOINT_xxx flags + BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0 + SimdScalar lambda[6]; // lambda generated by last step +}; + + +#endif //BU_Joint_H diff --git a/BulletDynamics/Dynamics/ContactJoint.cpp b/BulletDynamics/Dynamics/ContactJoint.cpp new file mode 100644 index 000000000..9fd859e0f --- /dev/null +++ b/BulletDynamics/Dynamics/ContactJoint.cpp @@ -0,0 +1,270 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "ContactJoint.h" +#include "RigidBody.h" +#include "NarrowPhaseCollision/PersistentManifold.h" + + +//this constant needs to be set up so different solvers give 'similar' results +#define FRICTION_CONSTANT 120.f + + +ContactJoint::ContactJoint(PersistentManifold* manifold,int index,bool swap,RigidBody* body0,RigidBody* body1) +:m_manifold(manifold), +m_index(index), +m_swapBodies(swap), +m_body0(body0), +m_body1(body1) +{ +} + +int m_numRows = 3; + + +void ContactJoint::GetInfo1(Info1 *info) +{ + info->m = m_numRows; + //friction adds another 2... + + info->nub = 0; +} + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + +#define M_SQRT12 SimdScalar(0.7071067811865475244008443621048490) + +#define dRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x)))) /* reciprocal square root */ + + + +void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q) +{ + if (SimdFabs(n[2]) > M_SQRT12) { + // choose p in y-z plane + SimdScalar a = n[1]*n[1] + n[2]*n[2]; + SimdScalar k = dRecipSqrt (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 + SimdScalar a = n[0]*n[0] + n[1]*n[1]; + SimdScalar k = dRecipSqrt (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; + } +} + + + +void ContactJoint::GetInfo2(Info2 *info) +{ + + int s = info->rowskip; + int s2 = 2*s; + + float swapFactor = m_swapBodies ? -1.f : 1.f; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + + + ManifoldPoint& point = m_manifold->GetContactPoint(m_index); + + normal[0] = swapFactor*point.m_normalWorldOnB[0]; + normal[1] = swapFactor*point.m_normalWorldOnB[1]; + normal[2] = swapFactor*point.m_normalWorldOnB[2]; + normal[3] = 0; // @@@ hmmm + + // if (GetBody0()) + SimdVector3 relativePositionA; + { + relativePositionA = point.GetPositionWorldOnA() - m_body0->getCenterOfMassPosition(); + dVector3 c1; + c1[0] = relativePositionA[0]; + c1[1] = relativePositionA[1]; + c1[2] = relativePositionA[2]; + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + + } + // if (GetBody1()) + SimdVector3 relativePositionB; + { + dVector3 c2; + relativePositionB = point.GetPositionWorldOnB() - m_body1->getCenterOfMassPosition(); + + // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - + // j->node[1].body->pos[i]; + c2[0] = relativePositionB[0]; + c2[1] = relativePositionB[1]; + c2[2] = relativePositionB[2]; + + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + + SimdScalar k = info->fps * info->erp; + + float depth = -point.GetDistance(); +// if (depth < 0.f) +// depth = 0.f; + + info->c[0] = k * depth; + //float maxvel = .2f; + +// if (info->c[0] > maxvel) +// info->c[0] = maxvel; + + + //can override it, not necessary +// info->cfm[0] = 0.f; +// info->cfm[1] = 0.f; +// info->cfm[2] = 0.f; + + + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = 1e30f;//dInfinity; + info->lo[1] = 0; + info->hi[1] = 0.f; + info->lo[2] = 0.f; + info->hi[2] = 0.f; + +#define DO_THE_FRICTION_2 +#ifdef DO_THE_FRICTION_2 + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + dVector3 c1; + c1[0] = relativePositionA[0]; + c1[1] = relativePositionA[1]; + c1[2] = relativePositionA[2]; + + dVector3 c2; + c2[0] = relativePositionB[0]; + c2[1] = relativePositionB[1]; + c2[2] = relativePositionB[2]; + + + float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction(); + + // first friction direction + if (m_numRows >= 2) + { + + + + dPlaneSpace1 (normal,t1,t2); + + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); + if (1) { //j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); + } + // set right hand side + if (0) {//j->contact.surface.mode & dContactMotion1) { + //info->c[1] = j->contact.surface.motion1; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + //1e30f + + + info->lo[1] = -friction;//-j->contact.surface.mu; + info->hi[1] = friction;//j->contact.surface.mu; + if (1)//j->contact.surface.mode & dContactApprox1_1) + info->findex[1] = 0; + + // set slip (constraint force mixing) + if (0)//j->contact.surface.mode & dContactSlip1) + { + // info->cfm[1] = j->contact.surface.slip1; + } else + { + //info->cfm[1] = 0.f; + } + } + + // second friction direction + if (m_numRows >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); + if (1) { //j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); + } + + // set right hand side + if (0){//j->contact.surface.mode & dContactMotion2) { + //info->c[2] = j->contact.surface.motion2; + } + // set LCP bounds and friction index. this depends on the approximation + // mode + if (0){//j->contact.surface.mode & dContactMu2) { + //info->lo[2] = -j->contact.surface.mu2; + //info->hi[2] = j->contact.surface.mu2; + } + else { + info->lo[2] = -friction; + info->hi[2] = friction; + } + if (0)//j->contact.surface.mode & dContactApprox1_2) + + { + info->findex[2] = 0; + } + // set slip (constraint force mixing) + if (0) //j->contact.surface.mode & dContactSlip2) + + { + //info->cfm[2] = j->contact.surface.slip2; + + } + } + +#endif //DO_THE_FRICTION_2 + +} + diff --git a/BulletDynamics/Dynamics/ContactJoint.h b/BulletDynamics/Dynamics/ContactJoint.h new file mode 100644 index 000000000..f8727d937 --- /dev/null +++ b/BulletDynamics/Dynamics/ContactJoint.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 CONTACT_JOINT_H +#define CONTACT_JOINT_H + +#include "BU_Joint.h" +class RigidBody; +class PersistentManifold; + +class ContactJoint : public BU_Joint +{ + PersistentManifold* m_manifold; + int m_index; + bool m_swapBodies; + RigidBody* m_body0; + RigidBody* m_body1; + + +public: + + ContactJoint() {}; + + ContactJoint(PersistentManifold* manifold,int index,bool swap,RigidBody* body0,RigidBody* body1); + + //BU_Joint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); + + + + +}; + +#endif //CONTACT_JOINT_H + diff --git a/BulletDynamics/Dynamics/MassProps.h b/BulletDynamics/Dynamics/MassProps.h new file mode 100644 index 000000000..b98010a68 --- /dev/null +++ b/BulletDynamics/Dynamics/MassProps.h @@ -0,0 +1,33 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 MASS_PROPS_H +#define MASS_PROPS_H + +#include + +struct MassProps { + MassProps(float mass,const SimdVector3& inertiaLocal): + m_mass(mass), + m_inertiaLocal(inertiaLocal) + { + } + float m_mass; + SimdVector3 m_inertiaLocal; +}; + + +#endif + diff --git a/BulletDynamics/Dynamics/RigidBody.cpp b/BulletDynamics/Dynamics/RigidBody.cpp new file mode 100644 index 000000000..e4ccf8395 --- /dev/null +++ b/BulletDynamics/Dynamics/RigidBody.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 "RigidBody.h" +#include "MassProps.h" +#include "CollisionShapes/ConvexShape.h" +#include "GEN_MinMax.h" +#include + +float gLinearAirDamping = 1.f; + +static int uniqueId = 0; + +RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar 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_friction(friction), + m_restitution(restitution), + m_kinematicTimeStep(0.f) +{ + m_debugBodyId = uniqueId++; + + setMassProps(massProps.m_mass, massProps.m_inertiaLocal); + setDamping(linearDamping, angularDamping); + m_worldTransform.setIdentity(); + updateInertiaTensor(); + +} + + +void RigidBody::setLinearVelocity(const SimdVector3& lin_vel) +{ + + m_linearVelocity = lin_vel; +} + + +void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const +{ + SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); +} + +void RigidBody::saveKinematicState(SimdScalar timeStep) +{ + + if (m_kinematicTimeStep) + { + SimdVector3 linVel,angVel; + SimdTransformUtil::CalculateVelocity(m_interpolationWorldTransform,m_worldTransform,m_kinematicTimeStep,m_linearVelocity,m_angularVelocity); + //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); + } + + + m_interpolationWorldTransform = m_worldTransform; + + m_kinematicTimeStep = timeStep; +} + +void RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const +{ + GetCollisionShape()->GetAabb(m_worldTransform,aabbMin,aabbMax); +} + + + + +void RigidBody::setGravity(const SimdVector3& acceleration) +{ + if (m_inverseMass != 0.0f) + { + m_gravity = acceleration * (1.0f / m_inverseMass); + } +} + + + + + + +void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping) +{ + m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f); + m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f); +} + + + +#include + + +void RigidBody::applyForces(SimdScalar step) +{ + if (IsStatic()) + 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) + { + SimdVector3 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) + { + SimdVector3 dir = m_angularVelocity.normalized(); + m_angularVelocity -= dir * angDampVel; + } else + { + m_angularVelocity.setValue(0.f,0.f,0.f); + } + } +#endif //FORCE_VELOCITY_DAMPING + +} + +void RigidBody::proceedToTransform(const SimdTransform& newTrans) +{ + setCenterOfMassTransform( newTrans ); +} + + +void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia) +{ + if (mass == 0.f) + { + m_collisionFlags = CollisionObject::isStatic; + m_inverseMass = 0.f; + } else + { + m_collisionFlags = 0; + 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 RigidBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); +} + + +void RigidBody::integrateVelocities(SimdScalar step) +{ + if (IsStatic()) + 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(); +} + +SimdQuaternion RigidBody::getOrientation() const +{ + SimdQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; +} + + +void RigidBody::setCenterOfMassTransform(const SimdTransform& xform) +{ + m_worldTransform = xform; + updateInertiaTensor(); +} + + + diff --git a/BulletDynamics/Dynamics/RigidBody.h b/BulletDynamics/Dynamics/RigidBody.h new file mode 100644 index 000000000..fd96dc99d --- /dev/null +++ b/BulletDynamics/Dynamics/RigidBody.h @@ -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. +*/ + +#ifndef RIGIDBODY_H +#define RIGIDBODY_H + +#include +#include +#include +#include "BroadphaseCollision/BroadphaseProxy.h" + + +#include "CollisionDispatch/CollisionObject.h" + +class CollisionShape; +struct MassProps; +typedef SimdScalar dMatrix3[4*3]; + +extern float gLinearAirDamping; +extern bool gUseEpa; + +#define MAX_RIGIDBODIES 8192 + + +/// RigidBody class for RigidBody Dynamics +/// +class RigidBody : public CollisionObject +{ +public: + + RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution); + + void proceedToTransform(const SimdTransform& newTrans); + + + /// continuous collision detection needs prediction + void predictIntegratedTransform(SimdScalar step, SimdTransform& predictedTransform) const; + + void saveKinematicState(SimdScalar step); + + + void applyForces(SimdScalar step); + + void setGravity(const SimdVector3& acceleration); + + void setDamping(SimdScalar lin_damping, SimdScalar ang_damping); + + inline const CollisionShape* GetCollisionShape() const { + return m_collisionShape; + } + + inline CollisionShape* GetCollisionShape() { + return m_collisionShape; + } + + void setMassProps(SimdScalar mass, const SimdVector3& inertia); + + SimdScalar getInvMass() const { return m_inverseMass; } + const SimdMatrix3x3& getInvInertiaTensorWorld() const { + return m_invInertiaTensorWorld; + } + + void integrateVelocities(SimdScalar step); + + void setCenterOfMassTransform(const SimdTransform& xform); + + void applyCentralForce(const SimdVector3& force) + { + m_totalForce += force; + } + + const SimdVector3& getInvInertiaDiagLocal() + { + return m_invInertiaLocal; + }; + + void setInvInertiaDiagLocal(const SimdVector3& diagInvInertia) + { + m_invInertiaLocal = diagInvInertia; + } + + void applyTorque(const SimdVector3& torque) + { + m_totalTorque += torque; + } + + void applyForce(const SimdVector3& force, const SimdVector3& rel_pos) + { + applyCentralForce(force); + applyTorque(rel_pos.cross(force)); + } + + void applyCentralImpulse(const SimdVector3& impulse) + { + m_linearVelocity += impulse * m_inverseMass; + } + + void applyTorqueImpulse(const SimdVector3& torque) + { + if (!IsStatic()) + m_angularVelocity += m_invInertiaTensorWorld * torque; + + } + + void applyImpulse(const SimdVector3& impulse, const SimdVector3& 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 SimdPoint3& getCenterOfMassPosition() const { + return m_worldTransform.getOrigin(); + } + SimdQuaternion getOrientation() const; + + const SimdTransform& getCenterOfMassTransform() const { + return m_worldTransform; + } + const SimdVector3& getLinearVelocity() const { + return m_linearVelocity; + } + const SimdVector3& getAngularVelocity() const { + return m_angularVelocity; + } + + + void setLinearVelocity(const SimdVector3& lin_vel); + void setAngularVelocity(const SimdVector3& ang_vel) { + if (!IsStatic()) + { + m_angularVelocity = ang_vel; + } + } + + SimdVector3 getVelocityInLocalPoint(const SimdVector3& 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 SimdVector3& v) + { + m_worldTransform.getOrigin() += v; + } + + + void getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const; + + + 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; + } + + + inline float ComputeImpulseDenominator(const SimdPoint3& pos, const SimdVector3& normal) const + { + SimdVector3 r0 = pos - getCenterOfMassPosition(); + + SimdVector3 c0 = (r0).cross(normal); + + SimdVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); + + return m_inverseMass + normal.dot(vec); + + } + + inline float ComputeAngularImpulseDenominator(const SimdVector3& axis) const + { + SimdVector3 vec = axis * getInvInertiaTensorWorld(); + return axis.dot(vec); + } + + + +private: + + SimdMatrix3x3 m_invInertiaTensorWorld; + SimdVector3 m_gravity; + SimdVector3 m_invInertiaLocal; + SimdVector3 m_totalForce; + SimdVector3 m_totalTorque; +// SimdQuaternion m_orn1; + + SimdVector3 m_linearVelocity; + + SimdVector3 m_angularVelocity; + + SimdScalar m_linearDamping; + SimdScalar m_angularDamping; + SimdScalar m_inverseMass; + + SimdScalar m_friction; + SimdScalar m_restitution; + + SimdScalar m_kinematicTimeStep; + + BroadphaseProxy* m_broadphaseProxy; + + + + + + +public: + const BroadphaseProxy* GetBroadphaseProxy() const + { + return m_broadphaseProxy; + } + BroadphaseProxy* GetBroadphaseProxy() + { + return m_broadphaseProxy; + } + void SetBroadphaseProxy(BroadphaseProxy* broadphaseProxy) + { + m_broadphaseProxy = broadphaseProxy; + } + + + /// for ode solver-binding + dMatrix3 m_R;//temp + dMatrix3 m_I; + dMatrix3 m_invI; + + int m_odeTag; + float m_padding[1024]; + SimdVector3 m_tacc;//temp + SimdVector3 m_facc; + + + int m_debugBodyId; +}; + + + +#endif diff --git a/BulletDynamics/Jamfile b/BulletDynamics/Jamfile new file mode 100644 index 000000000..b7df2012a --- /dev/null +++ b/BulletDynamics/Jamfile @@ -0,0 +1,7 @@ +SubDir TOP BulletDynamics ; + +Library bulletdynamics : + [ Wildcard ConstraintSolver : *.h *.cpp ] + [ Wildcard Dynamics : *.h *.cpp ] +; +Recurse InstallHeader : .h ; diff --git a/BulletLicense.txt b/BulletLicense.txt new file mode 100644 index 000000000..c3ec68c21 --- /dev/null +++ b/BulletLicense.txt @@ -0,0 +1,17 @@ +/* +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +Free for commercial use, but please mail bullet@erwincoumans.com to report projects, and join the forum at +www.continuousphysics.com/Bullet/phpBB2 diff --git a/Bullet_Faq.pdf b/Bullet_Faq.pdf new file mode 100644 index 000000000..c4e8da94f Binary files /dev/null and b/Bullet_Faq.pdf differ diff --git a/Bullet_User_Manual.pdf b/Bullet_User_Manual.pdf new file mode 100644 index 000000000..bc4683607 Binary files /dev/null and b/Bullet_User_Manual.pdf differ diff --git a/Demos/BasicSample/BasicSample.cpp b/Demos/BasicSample/BasicSample.cpp new file mode 100644 index 000000000..9cb82d52e --- /dev/null +++ b/Demos/BasicSample/BasicSample.cpp @@ -0,0 +1,793 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "CcdPhysicsEnvironment.h" +#include "CcdPhysicsController.h" +#include "MyMotionState.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/SphereShape.h" +#include "CollisionShapes/ConeShape.h" +#include "CollisionShapes/Simplex1to4Shape.h" +#include "CollisionShapes/EmptyShape.h" + +#include "Dynamics/RigidBody.h" +#include "CollisionDispatch/CollisionDispatcher.h" +#include "BroadphaseCollision/SimpleBroadphase.h" +#include "BroadphaseCollision/AxisSweep3.h" +#include "ConstraintSolver/Point2PointConstraint.h" +#include "ConstraintSolver/HingeConstraint.h" + +#include "quickprof.h" +#include "PrintfDebugDrawer.h" + + +#include "PHY_Pro.h" +#include //printf debugging + + +int getDebugMode() +{ + return IDebugDraw::DBG_ProfileTimings; +} + +//make up for lack of glut and camera +float eye[3]={10.f,0.f,0.f}; +int glutScreenWidth=640; +int glutScreenHeight=480; + + +#ifdef _DEBUG +const int numObjects = 22; +#else +const int numObjects = 120; +#endif + +const int maxNumObjects = 450; + +MyMotionState ms[maxNumObjects]; +CcdPhysicsController* physObjects[maxNumObjects] = {0,0,0,0}; +int shapeIndex[maxNumObjects]; +CcdPhysicsEnvironment* physicsEnvironmentPtr = 0; + + +#define CUBE_HALF_EXTENTS 1 + +#define EXTRA_HEIGHT -20.f +static const int numShapes = 4; + +CollisionShape* shapePtr[numShapes] = +{ + new BoxShape (SimdVector3(50,10,50)), + new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), + new SphereShape (CUBE_HALF_EXTENTS- 0.05f), + + //new ConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS), + //new BU_Simplex1to4(SimdPoint3(-1,-1,-1),SimdPoint3(1,-1,-1),SimdPoint3(-1,1,-1),SimdPoint3(0,0,1)), + + //new EmptyShape(), + + new BoxShape (SimdVector3(0.4f,1.f,0.8f)) + +}; + +void clientResetScene(); +void simulateMe(); +void clientDisplay() +{ +} +void clientMoveAndDisplay() +{ +} +PrintfDebugDrawer debugDrawer; + +int main(int argc,char** argv) +{ + + + + CollisionDispatcher* dispatcher = new CollisionDispatcher(); + + + SimdVector3 worldAabbMin(-10000,-10000,-10000); + SimdVector3 worldAabbMax(10000,10000,10000); + + BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); + //BroadphaseInterface* broadphase = new SimpleBroadphase(); + + physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); + physicsEnvironmentPtr->setDeactivationTime(2.f); + + + physicsEnvironmentPtr->setGravity(0,-10,0); + PHY_ShapeProps shapeProps; + + shapeProps.m_do_anisotropic = false; + shapeProps.m_do_fh = false; + shapeProps.m_do_rot_fh = false; + shapeProps.m_friction_scaling[0] = 1.; + shapeProps.m_friction_scaling[1] = 1.; + shapeProps.m_friction_scaling[2] = 1.; + + shapeProps.m_inertia = 1.f; + shapeProps.m_lin_drag = 0.2f; + shapeProps.m_ang_drag = 0.1f; + shapeProps.m_mass = 10.0f; + + PHY_MaterialProps materialProps; + materialProps.m_friction = 10.5f; + materialProps.m_restitution = 0.0f; + + CcdConstructionInfo ccdObjectCi; + ccdObjectCi.m_friction = 0.5f; + + ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; + ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; + + SimdTransform tr; + tr.setIdentity(); + + int i; + for (i=0;i0) + { + shapeIndex[i] = 1;//sphere + } + else + shapeIndex[i] = 0; + } + + + + + for (i=0;iSetMargin(0.05f); + + + + bool isDyna = i>0; + //if (i==1) + // isDyna=false; + + if (0)//i==1) + { + SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); + ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); + } + + + if (i>0) + { + + switch (i) + { + case 1: + { + ms[i].setWorldPosition(0,10,0); + //for testing, rotate the ground cube so the stack has to recover a bit + + break; + } + case 2: + { + ms[i].setWorldPosition(0,8,2); + break; + } + default: + ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); + } + + + SimdQuaternion quat; + SimdVector3 axis(0.f,0.f,1.f); + SimdScalar angle=0.5f; + + quat.setRotation(axis,angle); + + ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]); + + + + } else + { + ms[i].setWorldPosition(0,-100+EXTRA_HEIGHT,0); + + } + + ccdObjectCi.m_MotionState = &ms[i]; + ccdObjectCi.m_gravity = SimdVector3(0,0,0); + ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); + if (!isDyna) + { + shapeProps.m_mass = 0.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + else + { + shapeProps.m_mass = 1.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + + + SimdVector3 localInertia; + if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + //take inertia from first shape + shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } else + { + shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } + ccdObjectCi.m_localInertiaTensor = localInertia; + + ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; + + + physObjects[i]= new CcdPhysicsController( ccdObjectCi); + physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); + + if (i==1) + { + //physObjects[i]->SetAngularVelocity(0,0,-2,true); + } + + physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); + + } + + + //create a constraint + { + //physObjects[i]->SetAngularVelocity(0,0,-2,true); + int constraintId; + + //0.0f, -1.0f, 1.0f + + float pivotX=CUBE_HALF_EXTENTS, + pivotY=-CUBE_HALF_EXTENTS, + pivotZ=CUBE_HALF_EXTENTS; + + float axisX=1,axisY=0,axisZ=0; + + /*constraintId =physicsEnvironmentPtr->createConstraint( + physObjects[1], + //0, + physObjects[2], + //PHY_POINT2POINT_CONSTRAINT, + PHY_LINEHINGE_CONSTRAINT, + pivotX,pivotY,pivotZ, + axisX,axisY,axisZ + ); + + */ + + HingeConstraint* hinge = 0; + + SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 axisInA(0,1,0); + SimdVector3 axisInB(0,-1,0); + + RigidBody* rb0 = physObjects[1]->GetRigidBody(); + RigidBody* rb1 = physObjects[2]->GetRigidBody(); + + hinge = new HingeConstraint( + *rb0, + *rb1,pivotInA,pivotInB,axisInA,axisInB); + + physicsEnvironmentPtr->m_constraints.push_back(hinge); + + hinge->SetUserConstraintId(100); + hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT); + + } + + + + + clientResetScene(); + + while (1) + { + + simulateMe(); + } + + +} + +//to be implemented by the demo + + +void simulateMe() +{ + float deltaTime = 1.f/60.f; + physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime); + + + debugDrawer.SetDebugMode(getDebugMode()); + + //render the hinge axis + { + SimdVector3 color(1,0,0); + SimdVector3 dirLocal(0,1,0); + SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA); + SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB); + SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; + SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; + debugDrawer.DrawLine(from,from+dirWorldA,color); + debugDrawer.DrawLine(fromB,fromB+dirWorldB,color); + } + + float m[16]; + int i; + + + if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP) + { + //don't use Bullet, use quickstep + physicsEnvironmentPtr->setSolverType(0); + } else + { + //Bullet LCP solver + physicsEnvironmentPtr->setSolverType(1); + } + + + bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison); + + physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled); + + + + + for (i=0;iGetRigidBody()->GetActivationState() == 1) //active + { + if (i & 1) + { + wireColor += SimdVector3 (1.f,0.f,0.f); + } else + { + wireColor += SimdVector3 (.5f,0.f,0.f); + } + } + if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING + { + if (i & 1) + { + wireColor += SimdVector3 (0.f,1.f, 0.f); + } else + { + wireColor += SimdVector3 (0.f,0.5f,0.f); + } + } + + char extraDebug[125]; + sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); + physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); + + //GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); + + } + + + if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText)) + { + + + + + float xOffset = 10.f; + float yStart = 20.f; + + float yIncr = -2.f; + + char buf[124]; + + //glColor3f(0, 0, 0); + +#ifdef USE_QUICKPROF + if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings) + { + static int counter = 0; + counter++; + std::map::iterator iter; + for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter) + { + char blockTime[128]; + sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); + printf(blockTime); + + + } + } + + +#endif //USE_QUICKPROF + + + + +/* + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"mouse to interact"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"space to reset"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"c to show contact points"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"cursor keys and z,x to navigate"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"i to toggle simulation, s single step"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"q to quit"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"d to toggle deactivation"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,". to shoot primitive"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"h to toggle help text"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP); + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"m Bullet GJK = %i",!isSatEnabled); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"n Bullet LCP = %i",useBulletLCP); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + */ + + + + } + +} + + + +///make this positive to show stack falling from a distance +///this shows the penalty tresholds in action, springy/spungy look + + + +void clientResetScene() +{ + + int i; + for (i=0;i0) + { + + if ((getDebugMode() & IDebugDraw::DBG_NoHelpText)) + { + if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == BOX_SHAPE_PROXYTYPE) + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[2]); + } else + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); + } + + BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; + physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); + } + + //stack them + int colsize = 10; + int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS); + int row2 = row; + int col = (i)%(colsize)-colsize/2; + + + if (col>3) + { + col=11; + row2 |=1; + } + physObjects[i]->setPosition(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS, + row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0); + physObjects[i]->setOrientation(0,0,0,1); + physObjects[i]->SetLinearVelocity(0,0,0,false); + physObjects[i]->SetAngularVelocity(0,0,0,false); + } else + { + ms[i].setWorldPosition(0,-10-EXTRA_HEIGHT,0); + } + } +} + +void shootBox(const SimdVector3& destination) +{ + int i = numObjects-1; + + float speed = 40.f; + SimdVector3 linVel(destination[0]-eye[0],destination[1]-eye[1],destination[2]-eye[2]); + linVel.normalize(); + linVel*=speed; + + physObjects[i]->setPosition(eye[0],eye[1],eye[2]); + physObjects[i]->setOrientation(0,0,0,1); + physObjects[i]->SetLinearVelocity(linVel[0],linVel[1],linVel[2],false); + physObjects[i]->SetAngularVelocity(0,0,0,false); +} + +void clientKeyboard(unsigned char key, int x, int y) +{ + + if (key == '.') + { + shootBox(SimdVector3(0,0,0)); + } + //defaultKeyboard(key, x, y); +} + +int gPickingConstraintId = 0; +SimdVector3 gOldPickingPos; +float gOldPickingDist = 0.f; +RigidBody* pickedBody = 0;//for deactivation state + + +SimdVector3 GetRayTo(int x,int y) +{ + float top = 1.f; + float bottom = -1.f; + float nearPlane = 1.f; + float tanFov = (top-bottom)*0.5f / nearPlane; + float fov = 2.0 * atanf (tanFov); + + SimdVector3 rayFrom(eye[0],eye[1],eye[2]); + SimdVector3 rayForward = -rayFrom; + rayForward.normalize(); + float farPlane = 600.f; + rayForward*= farPlane; + + SimdVector3 rightOffset; + SimdVector3 vertical(0.f,1.f,0.f); + SimdVector3 hor; + hor = rayForward.cross(vertical); + hor.normalize(); + vertical = hor.cross(rayForward); + vertical.normalize(); + + float tanfov = tanf(0.5f*fov); + hor *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + SimdVector3 rayToCenter = rayFrom + rayForward; + SimdVector3 dHor = hor * 1.f/float(glutScreenWidth); + SimdVector3 dVert = vertical * 1.f/float(glutScreenHeight); + SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; + rayTo += x * dHor; + rayTo -= y * dVert; + return rayTo; +} +void clientMouseFunc(int button, int state, int x, int y) +{ + //printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y); + //button 0, state 0 means left mouse down + + SimdVector3 rayTo = GetRayTo(x,y); + + switch (button) + { + case 2: + { + if (state==0) + { + shootBox(rayTo); + } + break; + }; + case 1: + { + if (state==0) + { + //apply an impulse + if (physicsEnvironmentPtr) + { + float hit[3]; + float normal[3]; + PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]); + if (hitObj) + { + CcdPhysicsController* physCtrl = static_cast(hitObj); + RigidBody* body = physCtrl->GetRigidBody(); + if (body) + { + body->SetActivationState(ACTIVE_TAG); + SimdVector3 impulse = rayTo; + impulse.normalize(); + float impulseStrength = 10.f; + impulse *= impulseStrength; + SimdVector3 relPos( + hit[0] - body->getCenterOfMassPosition().getX(), + hit[1] - body->getCenterOfMassPosition().getY(), + hit[2] - body->getCenterOfMassPosition().getZ()); + + body->applyImpulse(impulse,relPos); + } + + } + + } + + } else + { + + } + break; + } + case 0: + { + if (state==0) + { + //add a point to point constraint for picking + if (physicsEnvironmentPtr) + { + float hit[3]; + float normal[3]; + PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]); + if (hitObj) + { + + CcdPhysicsController* physCtrl = static_cast(hitObj); + RigidBody* body = physCtrl->GetRigidBody(); + + if (body) + { + pickedBody = body; + pickedBody->SetActivationState(DISABLE_DEACTIVATION); + + SimdVector3 pickPos(hit[0],hit[1],hit[2]); + + SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + + gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT, + localPivot.getX(), + localPivot.getY(), + localPivot.getZ(), + 0,0,0); + //printf("created constraint %i",gPickingConstraintId); + + //save mouse position for dragging + gOldPickingPos = rayTo; + + + SimdVector3 eyePos(eye[0],eye[1],eye[2]); + + gOldPickingDist = (pickPos-eyePos).length(); + + Point2PointConstraint* p2p = static_cast(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); + if (p2p) + { + //very weak constraint for picking + p2p->m_setting.m_tau = 0.1f; + } + } + } + } + } else + { + if (gPickingConstraintId && physicsEnvironmentPtr) + { + physicsEnvironmentPtr->removeConstraint(gPickingConstraintId); + //printf("removed constraint %i",gPickingConstraintId); + gPickingConstraintId = 0; + pickedBody->ForceActivationState(ACTIVE_TAG); + pickedBody->m_deactivationTime = 0.f; + pickedBody = 0; + + + } + } + + break; + + } + default: + { + } + } + +} + +void clientMotionFunc(int x,int y) +{ + + if (gPickingConstraintId && physicsEnvironmentPtr) + { + + //move the constraint pivot + + Point2PointConstraint* p2p = static_cast(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); + if (p2p) + { + //keep it at the same picking distance + + SimdVector3 newRayTo = GetRayTo(x,y); + SimdVector3 eyePos(eye[0],eye[1],eye[2]); + SimdVector3 dir = newRayTo-eyePos; + dir.normalize(); + dir *= gOldPickingDist; + + SimdVector3 newPos = eyePos + dir; + p2p->SetPivotB(newPos); + } + + } +} diff --git a/Demos/BasicSample/Jamfile b/Demos/BasicSample/Jamfile new file mode 100644 index 000000000..65fec5333 --- /dev/null +++ b/Demos/BasicSample/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos BasicSample ; + +BulletBasicDemo BasicSample : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/BasicSample/MyMotionState.cpp b/Demos/BasicSample/MyMotionState.cpp new file mode 100644 index 000000000..956aac1ec --- /dev/null +++ b/Demos/BasicSample/MyMotionState.cpp @@ -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. +*/ + +#include "MyMotionState.h" +#include "SimdPoint3.h" + +MyMotionState::MyMotionState() +{ + m_worldTransform.setIdentity(); +} + + +MyMotionState::~MyMotionState() +{ + +} + +void MyMotionState::getWorldPosition(float& posX,float& posY,float& posZ) +{ + posX = m_worldTransform.getOrigin().x(); + posY = m_worldTransform.getOrigin().y(); + posZ = m_worldTransform.getOrigin().z(); +} + +void MyMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) +{ + scaleX = 1.; + scaleY = 1.; + scaleZ = 1.; +} + +void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) +{ + quatIma0 = m_worldTransform.getRotation().x(); + quatIma1 = m_worldTransform.getRotation().y(); + quatIma2 = m_worldTransform.getRotation().z(); + quatReal = m_worldTransform.getRotation()[3]; +} + +void MyMotionState::setWorldPosition(float posX,float posY,float posZ) +{ + SimdPoint3 pos(posX,posY,posZ); + m_worldTransform.setOrigin( pos ); +} + +void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) +{ + SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); + m_worldTransform.setRotation( orn ); +} + +void MyMotionState::calculateWorldTransformations() +{ + +} diff --git a/Demos/BasicSample/MyMotionState.h b/Demos/BasicSample/MyMotionState.h new file mode 100644 index 000000000..e1adb2bfa --- /dev/null +++ b/Demos/BasicSample/MyMotionState.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 MY_MOTIONSTATE_H +#define MY_MOTIONSTATE_H + +#include "PHY_IMotionState.h" +#include + + +class MyMotionState : public PHY_IMotionState + +{ + public: + MyMotionState(); + + virtual ~MyMotionState(); + + virtual void getWorldPosition(float& posX,float& posY,float& posZ); + virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ); + virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal); + + virtual void setWorldPosition(float posX,float posY,float posZ); + virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal); + + virtual void calculateWorldTransformations(); + + SimdTransform m_worldTransform; + +}; + +#endif //MY_MOTIONSTATE_H diff --git a/Demos/BasicSample/PrintfDebugDrawer.cpp b/Demos/BasicSample/PrintfDebugDrawer.cpp new file mode 100644 index 000000000..7e43fa468 --- /dev/null +++ b/Demos/BasicSample/PrintfDebugDrawer.cpp @@ -0,0 +1,31 @@ +#include "PrintfDebugDrawer.h" + +#include //printf debugging + +PrintfDebugDrawer::PrintfDebugDrawer() +{ +} + + +void PrintfDebugDrawer::DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color) +{ + if (m_debugMode > 0) + { + printf("DrawLine: from(%f , %f . %f) , to(%f , %f . %f)\n",from.getX(),from.getY(),from.getZ(),to.getX(),to.getY(),to.getZ()); + } +} + +void PrintfDebugDrawer::DrawContactPoint(const SimdVector3& pointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color) +{ + if (m_debugMode & IDebugDraw::DBG_DrawContactPoints) + { + SimdVector3 to=pointOnB+normalOnB*distance; + const SimdVector3&from = pointOnB; + printf("DrawContactPoint (%d) from(%f , %f . %f) , to(%f , %f . %f)\n",lifeTime, from.getX(),from.getY(),from.getZ(),to.getX(),to.getY(),to.getZ()); + } +} + +void PrintfDebugDrawer::SetDebugMode(int debugMode) +{ + m_debugMode = debugMode; +} diff --git a/Demos/BasicSample/PrintfDebugDrawer.h b/Demos/BasicSample/PrintfDebugDrawer.h new file mode 100644 index 000000000..499dff2b6 --- /dev/null +++ b/Demos/BasicSample/PrintfDebugDrawer.h @@ -0,0 +1,27 @@ +#ifndef PRINTF_DEBUG_DRAWER_H +#define PRINTF_DEBUG_DRAWER_H + +#include "IDebugDraw.h" + + + + +class PrintfDebugDrawer : public IDebugDraw +{ + int m_debugMode; + +public: + + PrintfDebugDrawer(); + + virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color); + + virtual void DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color); + + virtual void SetDebugMode(int debugMode); + + virtual int GetDebugMode() const { return m_debugMode;} + +}; + +#endif//PRINTF_DEBUG_DRAWER_H diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp new file mode 100644 index 000000000..d07c05390 --- /dev/null +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -0,0 +1,1305 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "CcdPhysicsEnvironment.h" +#include "CcdPhysicsController.h" +#include "MyMotionState.h" +//#include "GL_LineSegmentShape.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/SphereShape.h" +#include "CollisionShapes/ConeShape.h" + + +#include "CollisionShapes/Simplex1to4Shape.h" +#include "CollisionShapes/EmptyShape.h" + +#include "Dynamics/RigidBody.h" +#include "CollisionDispatch/CollisionDispatcher.h" +#include "BroadphaseCollision/SimpleBroadphase.h" +#include "BroadphaseCollision/AxisSweep3.h" +#include "ConstraintSolver/Point2PointConstraint.h" +#include "ConstraintSolver/HingeConstraint.h" + +#include "quickprof.h" +#include "IDebugDraw.h" + +#include "GLDebugDrawer.h" + +//#define COLLADA_PHYSICS_TEST 1 +#ifdef COLLADA_PHYSICS_TEST + +//Collada Physics test +//#define NO_LIBXML //need LIBXML, because FCDocument/FCDPhysicsRigidBody.h needs FUDaeWriter, through FCDPhysicsParameter.hpp +#include "FUtils/FUtils.h" +#include "FCDocument/FCDocument.h" +#include "FCDocument/FCDSceneNode.h" +#include "FUtils/FUFileManager.h" +#include "FUtils/FULogFile.h" +#include "FCDocument/FCDPhysicsSceneNode.h" +#include "FCDocument/FCDPhysicsModelInstance.h" +#include "FCDocument/FCDPhysicsRigidBodyInstance.h" +#include "FCDocument/FCDPhysicsRigidBody.h" +#include "FCDocument/FCDGeometryInstance.h" +#include "FCDocument/FCDGeometryMesh.h" +#include "FCDocument/FCDGeometry.h" +#include "FCDocument/FCDPhysicsAnalyticalGeometry.h" + + + + +//aaa + + + + + +#endif //COLLADA_PHYSICS_TEST + + + + +#include "PHY_Pro.h" +#include "BMF_Api.h" +#include //printf debugging + +float deltaTime = 1.f/60.f; +float bulletSpeed = 40.f; +bool createConstraint = true; +#ifdef WIN32 +#if _MSC_VER >= 1310 +//only use SIMD Hull code under Win32 +#define USE_HULL 1 +#include "NarrowPhaseCollision/Hull.h" +#endif //_MSC_VER +#endif //WIN32 + + +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GL_ShapeDrawer.h" + +#include "GlutStuff.h" + + +extern float eye[3]; +extern int glutScreenWidth; +extern int glutScreenHeight; + + +#ifdef _DEBUG +const int numObjects = 120;//22; +#else +const int numObjects = 120; +#endif + +const int maxNumObjects = 450; + +MyMotionState ms[maxNumObjects]; +CcdPhysicsController* physObjects[maxNumObjects] = {0,0,0,0}; +int shapeIndex[maxNumObjects]; +CcdPhysicsEnvironment* physicsEnvironmentPtr = 0; + + +#define CUBE_HALF_EXTENTS 1 + +#define EXTRA_HEIGHT -20.f +//GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0), +// SimdPoint3(50,0,0)); +static const int numShapes = 4; + +CollisionShape* shapePtr[numShapes] = +{ + ///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate. + ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346 + + new BoxShape (SimdVector3(450,10,450)), + new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), + new SphereShape (CUBE_HALF_EXTENTS- 0.05f), + + //new ConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS), + //new BU_Simplex1to4(SimdPoint3(-1,-1,-1),SimdPoint3(1,-1,-1),SimdPoint3(-1,1,-1),SimdPoint3(0,0,1)), + + //new EmptyShape(), + + new BoxShape (SimdVector3(0.4,1,0.8)) + +}; + + + +//////////////////////////////////// + +#ifdef COLLADA_PHYSICS_TEST + + +bool ConvertColladaPhysicsToBulletPhysics(const FCDPhysicsSceneNode* inputNode) +{ + + assert(inputNode); + + /// FRSceneNodeList nodesToDelete; + // FRMeshPhysicsController::StartCooking(); + + FCDPhysicsModelInstanceList models = inputNode->GetInstances(); + + //Go through all of the physics models + for (FCDPhysicsModelInstanceList::iterator itM=models.begin(); itM != models.end(); itM++) + { + + FCDEntityInstanceList& instanceList = (*itM)->GetInstances(); + //create one node per physics model. This node is pretty much only a middle man, + //but better describes the structure we get from the input file + //FRSceneNode* modelNode = new FRSceneNode(); + //modelNode->SetParent(outputNode); + //outputNode->AddChild(modelNode); + //Go through all of the rigid bodies and rigid constraints in that model + + for (FCDEntityInstanceList::iterator itE=instanceList.begin(); itE!=instanceList.end(); itE++) + { + if ((*itE)->GetType() == FCDEntityInstance::PHYSICS_RIGID_CONSTRAINT) + { + //not yet, could add point to point / hinge support easily + } + else + if ((*itE)->GetType() == FCDEntityInstance::PHYSICS_RIGID_BODY) + { + + printf("PHYSICS_RIGID_BODY\n"); + + + //Create a controller per rigid-body + + //FRMeshPhysicsController* controller = new FRMeshPhysicsController(inputNode->GetGravity(), inputNode->GetTimestep()); + FCDPhysicsRigidBodyInstance* rbInstance = (FCDPhysicsRigidBodyInstance*)(*itE); + + FCDSceneNode* targetNode = rbInstance->GetTargetNode(); + + + if (!targetNode) + { + + + //DebugOut("FCTranslator: No target node defined in rigid body instance"); + + //SAFE_DELETE(controller); + + continue; + } + + + //Transfer all the transforms in n into cNode, and bake + //at the same time the scalings. It is necessary to re-translate the + //transforms as they will get deleted when we delete the old node. + //A better way to do this would be to steal the transforms from the old + //nodes, and make sure they're not deleted later, but this is impractical + //right now as we would also have to migrate all the animation curves. + + + //FRTScaleList scaleTransforms; + + uint32 numTransforms = targetNode->GetTransformCount(); + + for (uint32 i=0; iGetTransforms()[i]); + } + + //Then affect all of its geometry instances. + //Collect all the entities inside the entity vector and inside the children nodes +/* + FREntityList childEntities = n->GetEntities(); + FRSceneNodeList childrenToParse = n->GetChildren(); + + while (!childrenToParse.empty()) + { + FRSceneNode* child = *childrenToParse.begin(); + const FREntityList& e = child->GetEntities(); + //add the entities of that child + childEntities.insert(childEntities.end(), e.begin(), e.end()); + //queue the grand-children nodes + childrenToParse.insert(childrenToParse.end(), child->GetChildren().begin(), child->GetChildren().end()); + childrenToParse.erase(childrenToParse.begin()); + + } +*/ + //now check which ones are geometry mesh (right now an entity is only derived by mesh + //but do this to avoid problems in the future) +/* + for (FREntityList::iterator itT = childEntities.begin(); itT != childEntities.end(); itT++) + { + + if ((*itT)->GetType() == FREntity::MESH || (*itT)->GetType() == FREntity::MESH_CONTROLLER) + + { + + FRMesh* cMesh = (FRMesh*)*itT; + + //while we're here, bake the scaling transforms into the meshes + + BakeScalingIntoMesh(cMesh, scaleTransforms); + + controller->AddBindMesh((FRMesh*)*itT); + + } + + } +*/ + + + ///////////////////////////////////////////////////////////////////// + //We're done with the targets. Now take care of the physics shapes. + FCDPhysicsRigidBody* rigidBody = rbInstance->FlattenRigidBody(); + FCDPhysicsMaterial* mat = rigidBody->GetPhysicsMaterial(); + FCDPhysicsShapeList shapes = rigidBody->GetPhysicsShapeList(); + for (uint32 i=0; iGetType();// + //controller->SetDensity(OldShape->GetDensity()); + + + if (OldShape->GetGeometryInstance()) + + { + + FCDGeometry* geoTemp = (FCDGeometry*)(OldShape->GetGeometryInstance()->GetEntity()); + + const FCDGeometryMesh* colladaMesh = geoTemp->GetMesh(); + + + + //FRMesh* cMesh = ToFREntityGeometry(geoTemp); + //BakeScalingIntoMesh(cMesh, scaleTransforms); + + for (uint32 j=0; jGetPolygonsCount(); j++) + { + /* + FRMeshPhysicsShape* NewShape = new FRMeshPhysicsShape(controller); + + if (!NewShape->CreateTriangleMesh(cMesh, j, true)) + + { + + SAFE_DELETE(NewShape); + + continue; + + } + if (mat) + { + NewShape->SetMaterial(mat->GetStaticFriction(), mat->GetDynamicFriction(), mat->GetRestitution()); + //FIXME + //NewShape->material->setFrictionCombineMode(); + //NewShape->material->setSpring(); + } + + controller->AddShape(NewShape); + + */ + } + + + + } + + else + + { + + //FRMeshPhysicsShape* NewShape = new FRMeshPhysicsShape(controller); + + FCDPhysicsAnalyticalGeometry* analGeom = OldShape->GetAnalyticalGeometry(); + + //increse the following value for nicer shapes with more vertices + + uint16 superEllipsoidSubdivisionLevel = 2; + + if (!analGeom) + + continue; + + switch (analGeom->GetGeomType()) + + { + + case FCDPhysicsAnalyticalGeometry::BOX: + + { + + FCDPASBox* box = (FCDPASBox*)analGeom; + + break; + + } + + case FCDPhysicsAnalyticalGeometry::PLANE: + + { + + FCDPASPlane* plane = (FCDPASPlane*)analGeom; + + break; + + } + + case FCDPhysicsAnalyticalGeometry::SPHERE: + + { + + FCDPASSphere* sphere = (FCDPASSphere*)analGeom; + + break; + + } + + case FCDPhysicsAnalyticalGeometry::CYLINDER: + + { + + //FIXME: only using the first radius of the cylinder + + FCDPASCylinder* cylinder = (FCDPASCylinder*)analGeom; + + + break; + + } + + case FCDPhysicsAnalyticalGeometry::CAPSULE: + + { + + //FIXME: only using the first radius of the capsule + + FCDPASCapsule* capsule = (FCDPASCapsule*)analGeom; + + + break; + + } + + case FCDPhysicsAnalyticalGeometry::TAPERED_CAPSULE: + + { + + //FIXME: only using the first radius of the capsule + + FCDPASTaperedCapsule* tcapsule = (FCDPASTaperedCapsule*)analGeom; + + break; + + } + + case FCDPhysicsAnalyticalGeometry::TAPERED_CYLINDER: + + { + + //FIXME: only using the first radius of the cylinder + + FCDPASTaperedCylinder* tcylinder = (FCDPASTaperedCylinder*)analGeom; + + break; + + } + + default: + + { + + break; + + } + + } + + //controller->AddShape(NewShape); + } + + } + + FCDPhysicsParameter* dyn = (FCDPhysicsParameter*)rigidBody->FindParameterByReference(DAE_DYNAMIC_ELEMENT); + + if (dyn) + { + //dyn->GetValue(); + } + + FCDPhysicsParameter* mass = (FCDPhysicsParameter*)rigidBody->FindParameterByReference(DAE_MASS_ELEMENT); + + if (mass) + { + mass->GetValue(); + } + + FCDPhysicsParameter* inertia = (FCDPhysicsParameter*)rigidBody->FindParameterByReference(DAE_INERTIA_ELEMENT); + + if (inertia) + { + inertia->GetValue(); + } + + FCDPhysicsParameter* velocity = (FCDPhysicsParameter*)rigidBody->FindParameterByReference(DAE_VELOCITY_ELEMENT); + + if (velocity) + { + velocity->GetValue(); + } + + FCDPhysicsParameter* angularVelocity = (FCDPhysicsParameter*)rigidBody->FindParameterByReference(DAE_ANGULAR_VELOCITY_ELEMENT); + + if (angularVelocity) + { + angularVelocity->GetValue(); + } + + //controller->SetGlobalPose(n->CalculateWorldTransformation());//?? + + //SAFE_DELETE(rigidBody); + + } + + } + + } + + return true; +} + + + +#endif //COLLADA_PHYSICS_TEST + + +//////////////////////////////////// + + + +GLDebugDrawer debugDrawer; + +int main(int argc,char** argv) +{ + +#ifdef COLLADA_PHYSICS_TEST + char* filename = "ColladaPhysics.dae"; + FCDocument* document = new FCDocument(); + FUStatus status = document->LoadFromFile(filename); + bool success = status.IsSuccessful(); + printf ("Collada import %i\n",success); + + if (success) + { + const FCDPhysicsSceneNode* physicsSceneRoot = document->GetPhysicsSceneRoot(); + if (ConvertColladaPhysicsToBulletPhysics( physicsSceneRoot )) + { + printf("ConvertColladaPhysicsToBulletPhysics successfull\n"); + } else + { + printf("ConvertColladaPhysicsToBulletPhysics failed\n"); + } + + + } +#endif //COLLADA_PHYSICS_TEST + + CollisionDispatcher* dispatcher = new CollisionDispatcher(); + + + SimdVector3 worldAabbMin(-10000,-10000,-10000); + SimdVector3 worldAabbMax(10000,10000,10000); + + BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); + //BroadphaseInterface* broadphase = new SimpleBroadphase(); + + physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); + physicsEnvironmentPtr->setDeactivationTime(2.f); + + physicsEnvironmentPtr->setGravity(0,-10,0); + PHY_ShapeProps shapeProps; + + shapeProps.m_do_anisotropic = false; + shapeProps.m_do_fh = false; + shapeProps.m_do_rot_fh = false; + shapeProps.m_friction_scaling[0] = 1.; + shapeProps.m_friction_scaling[1] = 1.; + shapeProps.m_friction_scaling[2] = 1.; + + shapeProps.m_inertia = 1.f; + shapeProps.m_lin_drag = 0.2f; + shapeProps.m_ang_drag = 0.1f; + shapeProps.m_mass = 10.0f; + + PHY_MaterialProps materialProps; + materialProps.m_friction = 10.5f; + materialProps.m_restitution = 0.0f; + + CcdConstructionInfo ccdObjectCi; + ccdObjectCi.m_friction = 0.5f; + + ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; + ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; + + SimdTransform tr; + tr.setIdentity(); + + int i; + for (i=0;i0) + { + shapeIndex[i] = 1;//sphere + } + else + shapeIndex[i] = 0; + } + + + + + for (i=0;iSetMargin(0.05f); + + + + bool isDyna = i>0; + //if (i==1) + // isDyna=false; + + if (0)//i==1) + { + SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); + ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); + } + + + if (i>0) + { + + switch (i) + { + case 1: + { + ms[i].setWorldPosition(0,10,0); + //for testing, rotate the ground cube so the stack has to recover a bit + + break; + } + case 2: + { + ms[i].setWorldPosition(0,8,2); + break; + } + default: + ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); + } + + float quatIma0,quatIma1,quatIma2,quatReal; + SimdQuaternion quat; + SimdVector3 axis(0,0,1); + SimdScalar angle=0.5f; + + quat.setRotation(axis,angle); + + ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]); + + + + } else + { + ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0); + + } + + ccdObjectCi.m_MotionState = &ms[i]; + ccdObjectCi.m_gravity = SimdVector3(0,0,0); + ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); + if (!isDyna) + { + shapeProps.m_mass = 0.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + ccdObjectCi.m_collisionFlags = CollisionObject::isStatic; + } + else + { + shapeProps.m_mass = 1.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + ccdObjectCi.m_collisionFlags = 0; + } + + + SimdVector3 localInertia; + if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + //take inertia from first shape + shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } else + { + shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } + ccdObjectCi.m_localInertiaTensor = localInertia; + + ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; + + + physObjects[i]= new CcdPhysicsController( ccdObjectCi); + + // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS + physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; + + //Experimental: better estimation of CCD Time of Impact: + //physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS; + + physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); + + if (i==1) + { + //physObjects[i]->SetAngularVelocity(0,0,-2,true); + } + + physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); + + } + + + //create a constraint + if (createConstraint) + { + //physObjects[i]->SetAngularVelocity(0,0,-2,true); + int constraintId; + + float pivotX=CUBE_HALF_EXTENTS, + pivotY=-CUBE_HALF_EXTENTS, + pivotZ=CUBE_HALF_EXTENTS; + + float axisX=1,axisY=0,axisZ=0; + + + + HingeConstraint* hinge = 0; + + SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 axisInA(0,1,0); + SimdVector3 axisInB(0,-1,0); + + RigidBody* rb0 = physObjects[1]->GetRigidBody(); + RigidBody* rb1 = physObjects[2]->GetRigidBody(); + + hinge = new HingeConstraint( + *rb0, + *rb1,pivotInA,pivotInB,axisInA,axisInB); + + physicsEnvironmentPtr->m_constraints.push_back(hinge); + + hinge->SetUserConstraintId(100); + hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT); + + } + + + + + clientResetScene(); + + setCameraDistance(26.f); + + return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/"); +} + +//to be implemented by the demo +void renderme() +{ + debugDrawer.SetDebugMode(getDebugMode()); + + //render the hinge axis + if (createConstraint) + { + SimdVector3 color(1,0,0); + SimdVector3 dirLocal(0,1,0); + SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); + SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA); + SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB); + SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; + SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; + debugDrawer.DrawLine(from,from+dirWorldA,color); + debugDrawer.DrawLine(fromB,fromB+dirWorldB,color); + } + + float m[16]; + int i; + + + if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP) + { + //don't use Bullet, use quickstep + physicsEnvironmentPtr->setSolverType(0); + } else + { + //Bullet LCP solver + physicsEnvironmentPtr->setSolverType(1); + } + + if (getDebugMode() & IDebugDraw::DBG_EnableCCD) + { + physicsEnvironmentPtr->setCcdMode(3); + } else + { + physicsEnvironmentPtr->setCcdMode(0); + } + + + bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison); + + physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled); + + +#ifdef USE_HULL + //some testing code for SAT + if (isSatEnabled) + { + for (int s=0;sIsPolyhedral()) + { + PolyhedralConvexShape* polyhedron = static_cast(shape); + if (!polyhedron->m_optionalHull) + { + //first convert vertices in 'Point3' format + int numPoints = polyhedron->GetNumVertices(); + Point3* points = new Point3[numPoints+1]; + //first 4 points should not be co-planar, so add central point to satisfy MakeHull + points[0] = Point3(0.f,0.f,0.f); + + SimdVector3 vertex; + for (int p=0;pGetVertex(p,vertex); + points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ()); + } + + Hull* hull = Hull::MakeHull(numPoints+1,points); + polyhedron->m_optionalHull = hull; + } + + } + } + + } +#endif //USE_HULL + + + for (i=0;iGetRigidBody()->GetActivationState() == 1) //active + { + if (i & 1) + { + wireColor += SimdVector3 (1.f,0.f,0.f); + } else + { + wireColor += SimdVector3 (.5f,0.f,0.f); + } + } + if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING + { + if (i & 1) + { + wireColor += SimdVector3 (0.f,1.f, 0.f); + } else + { + wireColor += SimdVector3 (0.f,0.5f,0.f); + } + } + + char extraDebug[125]; + sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); + physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); + GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); + + ///this block is just experimental code to show some internal issues with replacing shapes on the fly. + if (getDebugMode()!=0 && (i>0)) + { + if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); + + //remove the persistent collision pairs that were created based on the previous shape + + BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; + + physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); + + SimdVector3 newinertia; + SimdScalar newmass = 10.f; + physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia); + physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia); + physObjects[i]->GetRigidBody()->updateInertiaTensor(); + + } + + } + + + } + + if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText)) + { + + float xOffset = 10.f; + float yStart = 20.f; + + float yIncr = -2.f; + + char buf[124]; + + glColor3f(0, 0, 0); + +#ifdef USE_QUICKPROF + + + if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings) + { + static int counter = 0; + counter++; + std::map::iterator iter; + for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter) + { + char blockTime[128]; + sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); + glRasterPos3f(xOffset,yStart,0); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); + yStart += yIncr; + + } + } +#endif //USE_QUICKPROF + //profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT); + //<< std::endl; + + + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"mouse to interact"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"space to reset"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"cursor keys and z,x to navigate"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"i to toggle simulation, s single step"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"q to quit"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"d to toggle deactivation"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"a to draw temporal AABBs"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"h to toggle help text"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP); + + bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD); + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"m Bullet GJK = %i",!isSatEnabled); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"n Bullet LCP = %i",useBulletLCP); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + glRasterPos3f(xOffset,yStart,0); + sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + yStart += yIncr; + + } + +} + +void clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + + + physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime); + + renderme(); + + glFlush(); + glutSwapBuffers(); + +} + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + + physicsEnvironmentPtr->UpdateAabbs(deltaTime); + + renderme(); + + + glFlush(); + glutSwapBuffers(); +} + + + +///make this positive to show stack falling from a distance +///this shows the penalty tresholds in action, springy/spungy look + +void clientResetScene() +{ + + int i; + for (i=0;i0) + { + + if ((getDebugMode() & IDebugDraw::DBG_NoHelpText)) + { + if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == BOX_SHAPE_PROXYTYPE) + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[2]); + } else + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); + } + + BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; + physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); + } + + //stack them + int colsize = 10; + int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS); + int row2 = row; + int col = (i)%(colsize)-colsize/2; + + + if (col>3) + { + col=11; + row2 |=1; + } + physObjects[i]->setPosition(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS, + row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0); + physObjects[i]->setOrientation(0,0,0,1); + physObjects[i]->SetLinearVelocity(0,0,0,false); + physObjects[i]->SetAngularVelocity(0,0,0,false); + } + } +} + + + +void shootBox(const SimdVector3& destination) +{ + int i = numObjects-1; + + + + SimdVector3 linVel(destination[0]-eye[0],destination[1]-eye[1],destination[2]-eye[2]); + linVel.normalize(); + linVel*=bulletSpeed; + + physObjects[i]->setPosition(eye[0],eye[1],eye[2]); + physObjects[i]->setOrientation(0,0,0,1); + physObjects[i]->SetLinearVelocity(linVel[0],linVel[1],linVel[2],false); + physObjects[i]->SetAngularVelocity(0,0,0,false); +} + +void clientKeyboard(unsigned char key, int x, int y) +{ + + if (key == '.') + { + shootBox(SimdVector3(0,0,0)); + } + + if (key == '+') + { + bulletSpeed += 10.f; + } + if (key == '-') + { + bulletSpeed -= 10.f; + } + + defaultKeyboard(key, x, y); +} + +int gPickingConstraintId = 0; +SimdVector3 gOldPickingPos; +float gOldPickingDist = 0.f; +RigidBody* pickedBody = 0;//for deactivation state + + +SimdVector3 GetRayTo(int x,int y) +{ + float top = 1.f; + float bottom = -1.f; + float nearPlane = 1.f; + float tanFov = (top-bottom)*0.5f / nearPlane; + float fov = 2.0 * atanf (tanFov); + + SimdVector3 rayFrom(eye[0],eye[1],eye[2]); + SimdVector3 rayForward = -rayFrom; + rayForward.normalize(); + float farPlane = 600.f; + rayForward*= farPlane; + + SimdVector3 rightOffset; + SimdVector3 vertical(0.f,1.f,0.f); + SimdVector3 hor; + hor = rayForward.cross(vertical); + hor.normalize(); + vertical = hor.cross(rayForward); + vertical.normalize(); + + float tanfov = tanf(0.5f*fov); + hor *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + SimdVector3 rayToCenter = rayFrom + rayForward; + SimdVector3 dHor = hor * 1.f/float(glutScreenWidth); + SimdVector3 dVert = vertical * 1.f/float(glutScreenHeight); + SimdVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; + rayTo += x * dHor; + rayTo -= y * dVert; + return rayTo; +} +void clientMouseFunc(int button, int state, int x, int y) +{ + //printf("button %i, state %i, x=%i,y=%i\n",button,state,x,y); + //button 0, state 0 means left mouse down + + SimdVector3 rayTo = GetRayTo(x,y); + + switch (button) + { + case 2: + { + if (state==0) + { + shootBox(rayTo); + } + break; + }; + case 1: + { + if (state==0) + { + //apply an impulse + if (physicsEnvironmentPtr) + { + float hit[3]; + float normal[3]; + PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]); + if (hitObj) + { + CcdPhysicsController* physCtrl = static_cast(hitObj); + RigidBody* body = physCtrl->GetRigidBody(); + if (body) + { + body->SetActivationState(ACTIVE_TAG); + SimdVector3 impulse = rayTo; + impulse.normalize(); + float impulseStrength = 10.f; + impulse *= impulseStrength; + SimdVector3 relPos( + hit[0] - body->getCenterOfMassPosition().getX(), + hit[1] - body->getCenterOfMassPosition().getY(), + hit[2] - body->getCenterOfMassPosition().getZ()); + + body->applyImpulse(impulse,relPos); + } + + } + + } + + } else + { + + } + break; + } + case 0: + { + if (state==0) + { + //add a point to point constraint for picking + if (physicsEnvironmentPtr) + { + float hit[3]; + float normal[3]; + PHY_IPhysicsController* hitObj = physicsEnvironmentPtr->rayTest(0,eye[0],eye[1],eye[2],rayTo.getX(),rayTo.getY(),rayTo.getZ(),hit[0],hit[1],hit[2],normal[0],normal[1],normal[2]); + if (hitObj) + { + + CcdPhysicsController* physCtrl = static_cast(hitObj); + RigidBody* body = physCtrl->GetRigidBody(); + + if (body) + { + pickedBody = body; + pickedBody->SetActivationState(DISABLE_DEACTIVATION); + + SimdVector3 pickPos(hit[0],hit[1],hit[2]); + + SimdVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + + gPickingConstraintId = physicsEnvironmentPtr->createConstraint(physCtrl,0,PHY_POINT2POINT_CONSTRAINT, + localPivot.getX(), + localPivot.getY(), + localPivot.getZ(), + 0,0,0); + //printf("created constraint %i",gPickingConstraintId); + + //save mouse position for dragging + gOldPickingPos = rayTo; + + + SimdVector3 eyePos(eye[0],eye[1],eye[2]); + + gOldPickingDist = (pickPos-eyePos).length(); + + Point2PointConstraint* p2p = static_cast(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); + if (p2p) + { + //very weak constraint for picking + p2p->m_setting.m_tau = 0.1f; + } + } + } + } + } else + { + if (gPickingConstraintId && physicsEnvironmentPtr) + { + physicsEnvironmentPtr->removeConstraint(gPickingConstraintId); + //printf("removed constraint %i",gPickingConstraintId); + gPickingConstraintId = 0; + pickedBody->ForceActivationState(ACTIVE_TAG); + pickedBody->m_deactivationTime = 0.f; + pickedBody = 0; + + + } + } + + break; + + } + default: + { + } + } + +} + +void clientMotionFunc(int x,int y) +{ + + if (gPickingConstraintId && physicsEnvironmentPtr) + { + + //move the constraint pivot + + Point2PointConstraint* p2p = static_cast(physicsEnvironmentPtr->getConstraintById(gPickingConstraintId)); + if (p2p) + { + //keep it at the same picking distance + + SimdVector3 newRayTo = GetRayTo(x,y); + SimdVector3 eyePos(eye[0],eye[1],eye[2]); + SimdVector3 dir = newRayTo-eyePos; + dir.normalize(); + dir *= gOldPickingDist; + + SimdVector3 newPos = eyePos + dir; + p2p->SetPivotB(newPos); + } + + } +} diff --git a/Demos/CcdPhysicsDemo/Jamfile b/Demos/CcdPhysicsDemo/Jamfile new file mode 100644 index 000000000..6ff8d83fe --- /dev/null +++ b/Demos/CcdPhysicsDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos CcdPhysicsDemo ; + +BulletDemo CcdPhysicsDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/CcdPhysicsDemo/MyMotionState.cpp b/Demos/CcdPhysicsDemo/MyMotionState.cpp new file mode 100644 index 000000000..956aac1ec --- /dev/null +++ b/Demos/CcdPhysicsDemo/MyMotionState.cpp @@ -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. +*/ + +#include "MyMotionState.h" +#include "SimdPoint3.h" + +MyMotionState::MyMotionState() +{ + m_worldTransform.setIdentity(); +} + + +MyMotionState::~MyMotionState() +{ + +} + +void MyMotionState::getWorldPosition(float& posX,float& posY,float& posZ) +{ + posX = m_worldTransform.getOrigin().x(); + posY = m_worldTransform.getOrigin().y(); + posZ = m_worldTransform.getOrigin().z(); +} + +void MyMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) +{ + scaleX = 1.; + scaleY = 1.; + scaleZ = 1.; +} + +void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) +{ + quatIma0 = m_worldTransform.getRotation().x(); + quatIma1 = m_worldTransform.getRotation().y(); + quatIma2 = m_worldTransform.getRotation().z(); + quatReal = m_worldTransform.getRotation()[3]; +} + +void MyMotionState::setWorldPosition(float posX,float posY,float posZ) +{ + SimdPoint3 pos(posX,posY,posZ); + m_worldTransform.setOrigin( pos ); +} + +void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) +{ + SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); + m_worldTransform.setRotation( orn ); +} + +void MyMotionState::calculateWorldTransformations() +{ + +} diff --git a/Demos/CcdPhysicsDemo/MyMotionState.h b/Demos/CcdPhysicsDemo/MyMotionState.h new file mode 100644 index 000000000..e1adb2bfa --- /dev/null +++ b/Demos/CcdPhysicsDemo/MyMotionState.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 MY_MOTIONSTATE_H +#define MY_MOTIONSTATE_H + +#include "PHY_IMotionState.h" +#include + + +class MyMotionState : public PHY_IMotionState + +{ + public: + MyMotionState(); + + virtual ~MyMotionState(); + + virtual void getWorldPosition(float& posX,float& posY,float& posZ); + virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ); + virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal); + + virtual void setWorldPosition(float posX,float posY,float posZ); + virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal); + + virtual void calculateWorldTransformations(); + + SimdTransform m_worldTransform; + +}; + +#endif //MY_MOTIONSTATE_H diff --git a/Demos/CollisionDemo/CollisionDemo.cpp b/Demos/CollisionDemo/CollisionDemo.cpp new file mode 100644 index 000000000..c43fe2182 --- /dev/null +++ b/Demos/CollisionDemo/CollisionDemo.cpp @@ -0,0 +1,205 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +/// +/// Collision Demo shows a degenerate case, where the Simplex solver has to deal with near-affine dependent cases +/// See the define CATCH_DEGENERATE_TETRAHEDRON in Bullet's VoronoiSimplexSolver.cpp +/// + +#include "GL_Simplex1to4.h" +#include "SimdQuaternion.h" +#include "SimdTransform.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "CollisionShapes/BoxShape.h" + +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "NarrowPhaseCollision/PointCollector.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" + +#include "GL_ShapeDrawer.h" +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GlutStuff.h" + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 2; + +GL_Simplex1to4 simplex; + +PolyhedralConvexShape* shapePtr[maxNumObjects]; + +SimdTransform tr[numObjects]; +int screenWidth = 640.f; +int screenHeight = 480.f; + +void DrawRasterizerLine(float const* , float const*, int) +{ + +} + +int main(int argc,char** argv) +{ + setCameraDistance(20.f); + + tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f)); + tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f)); + + //tr[0].setOrigin(SimdVector3(0,0,0)); + //tr[1].setOrigin(SimdVector3(0,10,0)); + + SimdMatrix3x3 basisA; + basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, + -0.00029313788f,0.99753088f,0.070228584f, + -0.00089153741f,-0.070228823f,0.99753052f); + + SimdMatrix3x3 basisB; + basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, + 4.4865495e-018f,0.97979438f,0.20000751f, + 4.4410586e-017f,-0.20000751f,0.97979438f); + + tr[0].setBasis(basisA); + tr[1].setBasis(basisB); + + + + SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f); + SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f); + + BoxShape boxA(boxHalfExtentsA); + BoxShape boxB(boxHalfExtentsB); + shapePtr[0] = &boxA; + shapePtr[1] = &boxB; + + + SimdTransform tr; + tr.setIdentity(); + + + return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + + +static VoronoiSimplexSolver sGjkSimplexSolver; +SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glDisable(GL_LIGHTING); + + //GL_ShapeDrawer::DrawCoordSystem(); + + float m[16]; + int i; + + GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); + + SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f); + convexConvex.SetCachedSeperatingAxis(seperatingAxis); + + PointCollector gjkOutput; + GjkPairDetector::ClosestPointInput input; + input.m_transformA = tr[0]; + input.m_transformB = tr[1]; + + convexConvex.GetClosestPoints(input ,gjkOutput,0); + + if (gjkOutput.m_hasResult) + { + SimdVector3 endPt = gjkOutput.m_pointInWorld + + gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance; + + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z()); + glVertex3d(endPt.x(),endPt.y(),endPt.z()); + //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z()); + //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z()); + glEnd(); + + } + + for (i=0;i +#endif +#include +#include "GlutStuff.h" + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 2; + +GL_Simplex1to4 simplex; + + +CollisionObject objects[maxNumObjects]; +CollisionWorld* collisionWorld = 0; + +int screenWidth = 640.f; +int screenHeight = 480.f; + + +int main(int argc,char** argv) +{ + clientResetScene(); + + SimdMatrix3x3 basisA; + basisA.setIdentity(); + + SimdMatrix3x3 basisB; + basisB.setIdentity(); + + objects[0].m_worldTransform.setBasis(basisA); + objects[1].m_worldTransform.setBasis(basisB); + + SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1)}; + SimdPoint3 points1[5]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1),SimdPoint3(0,0,-1),SimdPoint3(-1,-1,0)}; + + BoxShape boxA(SimdVector3(1,1,1)); + BoxShape boxB(SimdVector3(0.5,0.5,0.5)); + //ConvexHullShape hullA(points0,3); + //hullA.setLocalScaling(SimdVector3(3,3,3)); + //ConvexHullShape hullB(points1,4); + //hullB.setLocalScaling(SimdVector3(4,4,4)); + + + objects[0].m_collisionShape = &boxA;//&hullA; + objects[1].m_collisionShape = &boxB;//&hullB; + + CollisionDispatcher dispatcher; + //SimpleBroadphase broadphase; + SimdVector3 worldAabbMin(-1000,-1000,-1000); + SimdVector3 worldAabbMax(1000,1000,1000); + + AxisSweep3 broadphase(worldAabbMin,worldAabbMax); + + collisionWorld = new CollisionWorld(&dispatcher,&broadphase); + + collisionWorld->AddCollisionObject(&objects[0]); + collisionWorld->AddCollisionObject(&objects[1]); + + return glutmain(argc, argv,screenWidth,screenHeight,"Collision Interface Demo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + + +static VoronoiSimplexSolver sGjkSimplexSolver; +SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glDisable(GL_LIGHTING); + + if (collisionWorld) + collisionWorld->PerformDiscreteCollisionDetection(); + + ///one way to draw all the contact points is iterating over contact manifolds / points: + int i; + int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds(); + for (i=0;iGetDispatcher()->GetManifoldByIndexInternal(i); + CollisionObject* obA = static_cast(contactManifold->GetBody0()); + CollisionObject* obB = static_cast(contactManifold->GetBody1()); + contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform); + + int numContacts = contactManifold->GetNumContacts(); + for (int j=0;jGetContactPoint(j); + + glBegin(GL_LINES); + glColor3f(1, 0, 1); + + SimdVector3 ptA = pt.GetPositionWorldOnA(); + SimdVector3 ptB = pt.GetPositionWorldOnB(); + + glVertex3d(ptA.x(),ptA.y(),ptA.z()); + glVertex3d(ptB.x(),ptB.y(),ptB.z()); + glEnd(); + } + } + + + //GL_ShapeDrawer::DrawCoordSystem(); + + float m[16]; + + + + for (i=0;i +#endif +#include +#include "GL_ShapeDrawer.h" + +#include "GlutStuff.h" + + +const int numObjects = 80; + +const int maxNumObjects = 100; +MyMotionState ms[maxNumObjects]; +CcdPhysicsController* physObjects[maxNumObjects] = {0,0,0,0}; +int shapeIndex[maxNumObjects]; +CcdPhysicsEnvironment* physicsEnvironmentPtr = 0; + +TriangleMesh meshData; +StridingMeshInterface* ptr; + + +//GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0), +// SimdPoint3(50,0,0)); +CollisionShape* shapePtr[5] = +{ + new BoxShape (SimdVector3(100,10,100)), + new BoxShape (SimdVector3(2,2,2)), + new BU_Simplex1to4(SimdPoint3(-2,-2,-2),SimdPoint3(2,-2,-2),SimdPoint3(-2,2,-2),SimdPoint3(0,0,2)), + + + new BoxShape (SimdVector3(1,3,1)), +#ifdef DEBUG_MESH + new TriangleMeshShape(&meshData), +#else + NULL, +#endif + //(&meshData) + +}; + +static const int NUM_VERTICES = 5; +static const int NUM_TRIANGLES=4; + +SimdVector3 gVertices[NUM_VERTICES]; +int gIndices[NUM_TRIANGLES*3]; + +int main(int argc,char** argv) +{ + + setCameraDistance(30.f); + +#define TRISIZE 10.f +#ifdef DEBUG_MESH + SimdVector3 vert0(-TRISIZE ,0,TRISIZE ); + SimdVector3 vert1(TRISIZE ,10,TRISIZE ); + SimdVector3 vert2(TRISIZE ,0,-TRISIZE ); + meshData.AddTriangle(vert0,vert1,vert2); + SimdVector3 vert3(-TRISIZE ,0,TRISIZE ); + SimdVector3 vert4(TRISIZE ,0,-TRISIZE ); + SimdVector3 vert5(-TRISIZE ,0,-TRISIZE ); + meshData.AddTriangle(vert3,vert4,vert5); +#else +#ifdef ODE_MESH + SimdVector3 Size = SimdVector3(15.f,15.f,12.5f); + + gVertices[0][0] = -Size[0]; + gVertices[0][1] = Size[2]; + gVertices[0][2] = -Size[1]; + + gVertices[1][0] = Size[0]; + gVertices[1][1] = Size[2]; + gVertices[1][2] = -Size[1]; + + gVertices[2][0] = Size[0]; + gVertices[2][1] = Size[2]; + gVertices[2][2] = Size[1]; + + gVertices[3][0] = -Size[0]; + gVertices[3][1] = Size[2]; + gVertices[3][2] = Size[1]; + + gVertices[4][0] = 0; + gVertices[4][1] = 0; + gVertices[4][2] = 0; + + gIndices[0] = 0; + gIndices[1] = 1; + gIndices[2] = 4; + + gIndices[3] = 1; + gIndices[4] = 2; + gIndices[5] = 4; + + gIndices[6] = 2; + gIndices[7] = 3; + gIndices[8] = 4; + + gIndices[9] = 3; + gIndices[10] = 0; + gIndices[11] = 4; + + int vertStride = sizeof(SimdVector3); + int indexStride = 3*sizeof(int); + + TriangleIndexVertexArray* indexVertexArrays = new TriangleIndexVertexArray(NUM_TRIANGLES, + gIndices, + indexStride, + NUM_VERTICES,(float*) &gVertices[0].x(),vertStride); + + //shapePtr[4] = new TriangleMeshShape(indexVertexArrays); + shapePtr[4] = new BvhTriangleMeshShape(indexVertexArrays); +#else + + int vertStride = sizeof(SimdVector3); + int indexStride = 3*sizeof(int); + + const int NUM_VERTS_X = 50; + const int NUM_VERTS_Y = 50; + const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y; + + const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); + + SimdVector3* gVertices = new SimdVector3[totalVerts]; + int* gIndices = new int[totalTriangles*3]; + + int i; + + for ( i=0;isetGravity(-1,-10,1); + PHY_ShapeProps shapeProps; + + shapeProps.m_do_anisotropic = false; + shapeProps.m_do_fh = false; + shapeProps.m_do_rot_fh = false; + shapeProps.m_friction_scaling[0] = 1.; + shapeProps.m_friction_scaling[1] = 1.; + shapeProps.m_friction_scaling[2] = 1.; + + shapeProps.m_inertia = 1.f; + shapeProps.m_lin_drag = 0.95999998f; + shapeProps.m_ang_drag = 0.89999998f; + shapeProps.m_mass = 1.0f; + + PHY_MaterialProps materialProps; + materialProps.m_friction = 0.f;// 50.5f; + materialProps.m_restitution = 0.1f; + + CcdConstructionInfo ccdObjectCi; + ccdObjectCi.m_friction = 0.f;//50.5f; + + ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; + ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; + + SimdTransform tr; + tr.setIdentity(); + + + for (i=0;i0) + shapeIndex[i] = 1;//2 = tetrahedron + else + shapeIndex[i] = 4; + } + for (i=0;i0; + + if (!i) + { + //SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); + //ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); + //ms[i].setWorldPosition(0,-10,0); + } else + { + ms[i].setWorldPosition(10,i*15-10,0); + } + +//either create a few stacks, to show several islands, or create 1 large stack, showing stability + //ms[i].setWorldPosition((i*5) % 30,i*15-10,0); + + + ccdObjectCi.m_MotionState = &ms[i]; + ccdObjectCi.m_gravity = SimdVector3(0,0,0); + ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); + if (!isDyna) + { + shapeProps.m_mass = 0.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + else + { + shapeProps.m_mass = 1.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + + + SimdVector3 localInertia; + if (shapeProps.m_mass>0.f) + { + shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } else + { + localInertia.setValue(0.f,0.f,0.f); + + } + ccdObjectCi.m_localInertiaTensor = localInertia; + + ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; + + + physObjects[i]= new CcdPhysicsController( ccdObjectCi); + physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); + +/* if (i==0) + { + physObjects[i]->SetAngularVelocity(0,0,-2,true); + physObjects[i]->GetRigidBody()->setDamping(0,0); + } +*/ + //for the line that represents the AABB extents +// physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); + + + } + return glutmain(argc, argv,640,480,"Static Concave Mesh Demo"); +} + + + +void renderme() +{ + float m[16]; + int i; + + for (i=0;iGetRigidBody()->GetActivationState() == 1) //active + { + wireColor = SimdVector3 (1.f,0.f,0.f); + } + if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING + { + wireColor = SimdVector3 (0.f,1.f,0.f); + } + + char extraDebug[125]; + //sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); + shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug); + GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode()); + } + +} +void clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + float deltaTime = 1.f/60.f; + + physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime); + + renderme(); + + glFlush(); + glutSwapBuffers(); + +} + + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + + glFlush(); + glutSwapBuffers(); +} + + +void clientResetScene() +{ + +} +void clientKeyboard(unsigned char key, int x, int y) +{ + defaultKeyboard(key, x, y); +} + + +void clientMouseFunc(int button, int state, int x, int y) +{ + +} +void clientMotionFunc(int x,int y) +{ +} \ No newline at end of file diff --git a/Demos/ConcaveDemo/Jamfile b/Demos/ConcaveDemo/Jamfile new file mode 100644 index 000000000..26b9182e9 --- /dev/null +++ b/Demos/ConcaveDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos ConcaveDemo ; + +BulletDemo ConcaveDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/ConcaveDemo/MyMotionState.cpp b/Demos/ConcaveDemo/MyMotionState.cpp new file mode 100644 index 000000000..956aac1ec --- /dev/null +++ b/Demos/ConcaveDemo/MyMotionState.cpp @@ -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. +*/ + +#include "MyMotionState.h" +#include "SimdPoint3.h" + +MyMotionState::MyMotionState() +{ + m_worldTransform.setIdentity(); +} + + +MyMotionState::~MyMotionState() +{ + +} + +void MyMotionState::getWorldPosition(float& posX,float& posY,float& posZ) +{ + posX = m_worldTransform.getOrigin().x(); + posY = m_worldTransform.getOrigin().y(); + posZ = m_worldTransform.getOrigin().z(); +} + +void MyMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) +{ + scaleX = 1.; + scaleY = 1.; + scaleZ = 1.; +} + +void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) +{ + quatIma0 = m_worldTransform.getRotation().x(); + quatIma1 = m_worldTransform.getRotation().y(); + quatIma2 = m_worldTransform.getRotation().z(); + quatReal = m_worldTransform.getRotation()[3]; +} + +void MyMotionState::setWorldPosition(float posX,float posY,float posZ) +{ + SimdPoint3 pos(posX,posY,posZ); + m_worldTransform.setOrigin( pos ); +} + +void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) +{ + SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); + m_worldTransform.setRotation( orn ); +} + +void MyMotionState::calculateWorldTransformations() +{ + +} diff --git a/Demos/ConcaveDemo/MyMotionState.h b/Demos/ConcaveDemo/MyMotionState.h new file mode 100644 index 000000000..e1adb2bfa --- /dev/null +++ b/Demos/ConcaveDemo/MyMotionState.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 MY_MOTIONSTATE_H +#define MY_MOTIONSTATE_H + +#include "PHY_IMotionState.h" +#include + + +class MyMotionState : public PHY_IMotionState + +{ + public: + MyMotionState(); + + virtual ~MyMotionState(); + + virtual void getWorldPosition(float& posX,float& posY,float& posZ); + virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ); + virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal); + + virtual void setWorldPosition(float posX,float posY,float posZ); + virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal); + + virtual void calculateWorldTransformations(); + + SimdTransform m_worldTransform; + +}; + +#endif //MY_MOTIONSTATE_H diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp new file mode 100644 index 000000000..9733627cd --- /dev/null +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -0,0 +1,388 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 "CcdPhysicsEnvironment.h" +#include "CcdPhysicsController.h" +#include "MyMotionState.h" +//#include "GL_LineSegmentShape.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/Simplex1to4Shape.h" +#include "CollisionShapes/EmptyShape.h" + +#include "Dynamics/RigidBody.h" +#include "ConstraintSolver/SimpleConstraintSolver.h" +#include "ConstraintSolver/OdeConstraintSolver.h" +#include "CollisionDispatch/CollisionDispatcher.h" +#include "BroadphaseCollision/SimpleBroadphase.h" +#include "IDebugDraw.h" + +#include "GLDebugDrawer.h" + +#include "PHY_Pro.h" +#include "BMF_Api.h" +#include //printf debugging + +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GL_ShapeDrawer.h" + +#include "GlutStuff.h" + +const int numObjects = 3; + +const int maxNumObjects = 400; +MyMotionState ms[maxNumObjects]; +CcdPhysicsController* physObjects[maxNumObjects] = {0,0,0,0}; +int shapeIndex[maxNumObjects]; +CcdPhysicsEnvironment* physicsEnvironmentPtr = 0; + + +#define CUBE_HALF_EXTENTS 1 + +//GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0), +// SimdPoint3(50,0,0)); +CollisionShape* shapePtr[4] = +{ + new BoxShape (SimdVector3(10,10,10)), + new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), + //new EmptyShape(), + new BU_Simplex1to4(SimdPoint3(-1,-1,-1),SimdPoint3(1,-1,-1),SimdPoint3(-1,1,-1),SimdPoint3(0,0,1)), + //new BoxShape (SimdVector3(0.4,1,0.8)) + +}; + + + GLDebugDrawer debugDrawer; + +int main(int argc,char** argv) +{ + + + ConstraintSolver* solver = new SimpleConstraintSolver; + //ConstraintSolver* solver = new OdeConstraintSolver; + + CollisionDispatcher* dispatcher = new CollisionDispatcher(); + + BroadphaseInterface* broadphase = new SimpleBroadphase(); + + + physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); + physicsEnvironmentPtr->setDeactivationTime(0.f); + + + physicsEnvironmentPtr->setGravity(0,-10,0); + PHY_ShapeProps shapeProps; + + shapeProps.m_do_anisotropic = false; + shapeProps.m_do_fh = false; + shapeProps.m_do_rot_fh = false; + shapeProps.m_friction_scaling[0] = 1.; + shapeProps.m_friction_scaling[1] = 1.; + shapeProps.m_friction_scaling[2] = 1.; + + shapeProps.m_inertia = 1.f; + shapeProps.m_lin_drag = 0.2f; + shapeProps.m_ang_drag = 0.1f; + shapeProps.m_mass = 10.0f; + + PHY_MaterialProps materialProps; + materialProps.m_friction = 10.5f; + materialProps.m_restitution = 0.0f; + + CcdConstructionInfo ccdObjectCi; + ccdObjectCi.m_friction = 0.5f; + + ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; + ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; + + SimdTransform tr; + tr.setIdentity(); + + int i; + for (i=0;i0) + shapeIndex[i] = 1;//2 to test start with EmptyShape + else + shapeIndex[i] = 0; + } + for (i=0;iSetMargin(0.05f); + + + bool isDyna = i>0; + + if (0)//i==1) + { + SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); + ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); + } + + + if (i>0) + { + ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); + } else + { + ms[i].setWorldPosition(0,-10,0); + + } + + ccdObjectCi.m_MotionState = &ms[i]; + ccdObjectCi.m_gravity = SimdVector3(0,0,0); + ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); + if (!isDyna) + { + shapeProps.m_mass = 0.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + else + { + shapeProps.m_mass = 1.f; + ccdObjectCi.m_mass = shapeProps.m_mass; + } + + + SimdVector3 localInertia; + if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + //take inertia from first shape + shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } else + { + shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); + } + ccdObjectCi.m_localInertiaTensor = localInertia; + + ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; + + + physObjects[i]= new CcdPhysicsController( ccdObjectCi); + physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); + + + + physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); + + } + + clientResetScene(); + { + //physObjects[i]->SetAngularVelocity(0,0,-2,true); + int constraintId; + + float pivotX=CUBE_HALF_EXTENTS, + pivotY=CUBE_HALF_EXTENTS, + pivotZ=CUBE_HALF_EXTENTS; + float axisX=0,axisY=1,axisZ=0; + + + constraintId =physicsEnvironmentPtr->createConstraint( + physObjects[1], + //0, + physObjects[2], + PHY_POINT2POINT_CONSTRAINT, + //PHY_LINEHINGE_CONSTRAINT, + pivotX,pivotY,pivotZ, + axisX,axisY,axisZ + ); + + } + + setCameraDistance(46.f); + + return glutmain(argc, argv,640,480,"Constraint Demo. http://www.continuousphysics.com/Bullet/phpBB2/"); +} + +//to be implemented by the demo + + +void renderme() +{ + debugDrawer.SetDebugMode(getDebugMode()); + + + float m[16]; + int i; + + for (i=0;iGetRigidBody()->GetActivationState() == 1) //active + { + wireColor = SimdVector3 (1.f,0.f,0.f); + } + if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING + { + wireColor = SimdVector3 (0.f,1.f,0.f); + } + + char extraDebug[125]; + sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); + physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); + GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); + + if (getDebugMode()!=0 && (i>0)) + { + if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) + { + physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); + + //remove the persistent collision pairs that were created based on the previous shape + + BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; + + physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); + + SimdVector3 newinertia; + SimdScalar newmass = 10.f; + physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia); + physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia); + physObjects[i]->GetRigidBody()->updateInertiaTensor(); + + } + + } + + + } + + glRasterPos3f(20,20,0); + char buf[124]; + sprintf(buf,"space to reset"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + glRasterPos3f(20,15,0); + sprintf(buf,"c to show contact points"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + glRasterPos3f(20,10,0); + sprintf(buf,"cursor keys and z,x to navigate"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + glRasterPos3f(20,5,0); + sprintf(buf,"i to toggle simulation, s single step"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + + glRasterPos3f(20,0,0); + sprintf(buf,"q to quit"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + glRasterPos3f(20,-5,0); + sprintf(buf,"d to toggle deactivation"); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + +} +void clientMoveAndDisplay() +{ + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + float deltaTime = 1.f/60.f; + + physicsEnvironmentPtr->proceedDeltaTime(0.f,deltaTime); + + renderme(); + + glFlush(); + glutSwapBuffers(); + +} + + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + renderme(); + + + + glFlush(); + glutSwapBuffers(); +} + + + +///make this positive to show stack falling from a distance +///this shows the penalty tresholds in action, springy/spungy look +#define EXTRA_HEIGHT 20.f + + +void clientResetScene() +{ + int i; + for (i=0;i0) + { + //stack them + int colsize = 10; + int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS); + int col = (i)%(colsize)-colsize/2; + + physObjects[i]->setPosition(col*2*CUBE_HALF_EXTENTS + (row%2)*CUBE_HALF_EXTENTS, + row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT, + 0); + physObjects[i]->setOrientation(0,0,0,1); + physObjects[i]->SetLinearVelocity(0,0,0,false); + physObjects[i]->SetAngularVelocity(0,0,0,false); + } else + { + ms[i].setWorldPosition(0,-10,0); + + } + } +} + +void clientKeyboard(unsigned char key, int x, int y) +{ + defaultKeyboard(key, x, y); +} + + +void clientMouseFunc(int button, int state, int x, int y) +{ + +} +void clientMotionFunc(int x,int y) +{ +} \ No newline at end of file diff --git a/Demos/ConstraintDemo/Jamfile b/Demos/ConstraintDemo/Jamfile new file mode 100644 index 000000000..e66af032e --- /dev/null +++ b/Demos/ConstraintDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos ConstraintDemo ; + +BulletDemo ConstraintDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/ConstraintDemo/MyMotionState.cpp b/Demos/ConstraintDemo/MyMotionState.cpp new file mode 100644 index 000000000..956aac1ec --- /dev/null +++ b/Demos/ConstraintDemo/MyMotionState.cpp @@ -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. +*/ + +#include "MyMotionState.h" +#include "SimdPoint3.h" + +MyMotionState::MyMotionState() +{ + m_worldTransform.setIdentity(); +} + + +MyMotionState::~MyMotionState() +{ + +} + +void MyMotionState::getWorldPosition(float& posX,float& posY,float& posZ) +{ + posX = m_worldTransform.getOrigin().x(); + posY = m_worldTransform.getOrigin().y(); + posZ = m_worldTransform.getOrigin().z(); +} + +void MyMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ) +{ + scaleX = 1.; + scaleY = 1.; + scaleZ = 1.; +} + +void MyMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal) +{ + quatIma0 = m_worldTransform.getRotation().x(); + quatIma1 = m_worldTransform.getRotation().y(); + quatIma2 = m_worldTransform.getRotation().z(); + quatReal = m_worldTransform.getRotation()[3]; +} + +void MyMotionState::setWorldPosition(float posX,float posY,float posZ) +{ + SimdPoint3 pos(posX,posY,posZ); + m_worldTransform.setOrigin( pos ); +} + +void MyMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal) +{ + SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal); + m_worldTransform.setRotation( orn ); +} + +void MyMotionState::calculateWorldTransformations() +{ + +} diff --git a/Demos/ConstraintDemo/MyMotionState.h b/Demos/ConstraintDemo/MyMotionState.h new file mode 100644 index 000000000..e1adb2bfa --- /dev/null +++ b/Demos/ConstraintDemo/MyMotionState.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 MY_MOTIONSTATE_H +#define MY_MOTIONSTATE_H + +#include "PHY_IMotionState.h" +#include + + +class MyMotionState : public PHY_IMotionState + +{ + public: + MyMotionState(); + + virtual ~MyMotionState(); + + virtual void getWorldPosition(float& posX,float& posY,float& posZ); + virtual void getWorldScaling(float& scaleX,float& scaleY,float& scaleZ); + virtual void getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal); + + virtual void setWorldPosition(float posX,float posY,float posZ); + virtual void setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal); + + virtual void calculateWorldTransformations(); + + SimdTransform m_worldTransform; + +}; + +#endif //MY_MOTIONSTATE_H diff --git a/Demos/ContinuousConvexCollision/ContinuousConvexCollisionDemo.cpp b/Demos/ContinuousConvexCollision/ContinuousConvexCollisionDemo.cpp new file mode 100644 index 000000000..58e6cd125 --- /dev/null +++ b/Demos/ContinuousConvexCollision/ContinuousConvexCollisionDemo.cpp @@ -0,0 +1,310 @@ +/* + * 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. + */ + + +/* + Continuous Convex Collision Demo demonstrates an efficient continuous collision detection algorithm. + Both linear and angular velocities are supported. Convex Objects are sampled using Supporting Vertex. + Motion using Exponential Map. + Future ideas: Comparison with Screwing Motion. + Also comparision with Algebraic CCD and Interval Arithmetic methods (Stephane Redon) +*/ + +#include "SimdQuaternion.h" +#include "SimdTransform.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" + +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "NarrowPhaseCollision/GjkConvexCast.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "NarrowPhaseCollision/ContinuousConvexCollision.h" + +#include "SimdTransformUtil.h" +#include "DebugCastResult.h" + +#include "CollisionShapes/SphereShape.h" + +#include "CollisionShapes/Simplex1to4Shape.h" + +#include "GL_ShapeDrawer.h" +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GlutStuff.h" + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 2; + +SimdVector3 angVels[numObjects]; +SimdVector3 linVels[numObjects]; + +PolyhedralConvexShape* shapePtr[maxNumObjects]; + + +SimdTransform fromTrans[maxNumObjects]; +SimdTransform toTrans[maxNumObjects]; + +//SimdTransform tr[numObjects]; + +void DrawRasterizerLine(float const* , float const*, int) +{ + +} +int screenWidth = 640.f; +int screenHeight = 480.f; + + +int main(int argc,char** argv) +{ + + setCameraDistance(40.f); + + fromTrans[0].setOrigin(SimdVector3(0,10,20)); + toTrans[0].setOrigin(SimdVector3(0,10,-20)); + fromTrans[1].setOrigin(SimdVector3(-2,7,0)); + toTrans[1].setOrigin(SimdVector3(-2,10,0)); + + SimdMatrix3x3 identBasis; + identBasis.setIdentity(); + + SimdMatrix3x3 basisA; + basisA.setIdentity(); + basisA.setEulerZYX(0.f,-SIMD_HALF_PI,0.f); + + fromTrans[0].setBasis(identBasis); + toTrans[0].setBasis(basisA); + + fromTrans[1].setBasis(identBasis); + toTrans[1].setBasis(identBasis); + + toTrans[1].setBasis(identBasis); + SimdVector3 boxHalfExtentsA(10,1,1); + SimdVector3 boxHalfExtentsB(1.1f,1.1f,1.1f); + BoxShape boxA(boxHalfExtentsA); +// BU_Simplex1to4 boxA(SimdPoint3(-2,0,-2),SimdPoint3(2,0,-2),SimdPoint3(0,0,2),SimdPoint3(0,2,0)); +// BU_Simplex1to4 boxA(SimdPoint3(-12,0,0),SimdPoint3(12,0,0)); + + + BoxShape boxB(boxHalfExtentsB); +// BU_Simplex1to4 boxB(SimdPoint3(0,10,0),SimdPoint3(0,-10,0)); + + + shapePtr[0] = &boxA; + shapePtr[1] = &boxB; + + shapePtr[0]->SetMargin(0.01f); + shapePtr[1]->SetMargin(0.01f); + + for (int i=0;iGetAngularMotionDisc(); + SimdScalar angspeed = angVels[i].length() * boundingRadius * dt; + SimdScalar linspeed = linVels[i].length() * dt; + + SimdScalar totalspeed = angspeed + linspeed; + + //for each object, subdivide the from/to transform in 10 equal steps + + int numSubSteps = 10.f; + for (int s=0;s<10;s++) + { + SimdScalar subStep = s * 1.f/(float)numSubSteps; + SimdTransform interpolatedTrans; + + SimdTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans); + + //fromTrans[i].getOpenGLMatrix(m); + //GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]); + + //toTrans[i].getOpenGLMatrix(m); + //GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i]); + + interpolatedTrans.getOpenGLMatrix( m ); + GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,0,1),getDebugMode()); + } + } + } + + + int shapeIndex = 1; + + SimdMatrix3x3 mat; + mat.setEulerZYX(yaw,pitch,roll); + SimdQuaternion orn; + mat.getRotation(orn); + orn.setEuler(yaw,pitch,roll); + fromTrans[1].setRotation(orn); + toTrans[1].setRotation(orn); + + extern bool stepping; + extern bool singleStep; + + if (stepping || singleStep) + { + singleStep = false; + pitch += 0.005f; +// yaw += 0.01f; + } +// SimdVector3 fromA(-25,11,0); +// SimdVector3 toA(-15,11,0); + +// SimdQuaternion ornFromA(0.f,0.f,0.f,1.f); +// SimdQuaternion ornToA(0.f,0.f,0.f,1.f); + +// SimdTransform rayFromWorld(ornFromA,fromA); +// SimdTransform rayToWorld(ornToA,toA); + + SimdTransform rayFromWorld = fromTrans[0]; + SimdTransform rayToWorld = toTrans[0]; + + + if (drawLine) + { + glBegin(GL_LINES); + glColor3f(0, 0, 1); + glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z()); + glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z()); + glEnd(); + } + + //now perform a raycast on the shapes, in local (shape) space + gGjkSimplexSolver.reset(); + + //choose one of the following lines + + + for (i=0;im_fraction,hitTrans); + + hitTrans.getOpenGLMatrix(m); + GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,0),getDebugMode()); + + SimdTransformUtil::IntegrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans); + + hitTrans.getOpenGLMatrix(m); + GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(0,1,1),getDebugMode()); + + + } + } + + glFlush(); + glutSwapBuffers(); +} + +void clientResetScene() +{ +} + +void clientKeyboard(unsigned char key, int x, int y) +{ + defaultKeyboard(key, x, y); +} + + +void clientMouseFunc(int button, int state, int x, int y) +{ + +} +void clientMotionFunc(int x,int y) +{ +} \ No newline at end of file diff --git a/Demos/ContinuousConvexCollision/Jamfile b/Demos/ContinuousConvexCollision/Jamfile new file mode 100644 index 000000000..be75e55d6 --- /dev/null +++ b/Demos/ContinuousConvexCollision/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos ContinuousConvexCollision ; + +BulletDemo ContinuousConvexCollision : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp b/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp new file mode 100644 index 000000000..9f158d3a4 --- /dev/null +++ b/Demos/ConvexHullDistance/ConvexHullDistanceDemo.cpp @@ -0,0 +1,188 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +/// +/// Convex Hull Distance Demo shows distance calculation between two convex hulls of points. +/// GJK with the VoronoiSimplexSolver is used. +/// + +#include "GL_Simplex1to4.h" +#include "SimdQuaternion.h" +#include "SimdTransform.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "CollisionShapes/ConvexHullShape.h" + +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "NarrowPhaseCollision/PointCollector.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" + +#include "GL_ShapeDrawer.h" +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GlutStuff.h" + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 2; + +GL_Simplex1to4 simplex; + +PolyhedralConvexShape* shapePtr[maxNumObjects]; + +SimdTransform tr[numObjects]; +int screenWidth = 640.f; +int screenHeight = 480.f; + + +int main(int argc,char** argv) +{ + clientResetScene(); + + SimdMatrix3x3 basisA; + basisA.setIdentity(); + + SimdMatrix3x3 basisB; + basisB.setIdentity(); + + tr[0].setBasis(basisA); + tr[1].setBasis(basisB); + + SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1)}; + SimdPoint3 points1[5]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1),SimdPoint3(0,0,-1),SimdPoint3(-1,-1,0)}; + + ConvexHullShape hullA(points0,3); + ConvexHullShape hullB(points1,5); + + shapePtr[0] = &hullA; + shapePtr[1] = &hullB; + + + SimdTransform tr; + tr.setIdentity(); + + + return glutmain(argc, argv,screenWidth,screenHeight,"Convex Hull Distance Demo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + + +static VoronoiSimplexSolver sGjkSimplexSolver; +SimplexSolverInterface& gGjkSimplexSolver = sGjkSimplexSolver; + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glDisable(GL_LIGHTING); + + //GL_ShapeDrawer::DrawCoordSystem(); + + float m[16]; + int i; + + GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); + + SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f); + convexConvex.SetCachedSeperatingAxis(seperatingAxis); + + PointCollector gjkOutput; + GjkPairDetector::ClosestPointInput input; + input.m_transformA = tr[0]; + input.m_transformB = tr[1]; + + convexConvex.GetClosestPoints(input ,gjkOutput,0); + + if (gjkOutput.m_hasResult) + { + SimdVector3 endPt = gjkOutput.m_pointInWorld + + gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance; + + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z()); + glVertex3d(endPt.x(),endPt.y(),endPt.z()); + glEnd(); + + } + + for (i=0;i +#include "GlutStuff.h" + +#include +#include +#include + +#include "CollisionShapes/ConvexShape.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/SphereShape.h" + +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" + +#include "NarrowPhaseCollision/EpaCommon.h" +#include "NarrowPhaseCollision/EpaVertex.h" +#include "NarrowPhaseCollision/EpaHalfEdge.h" +#include "NarrowPhaseCollision/EpaFace.h" +#include "NarrowPhaseCollision/EpaPolyhedron.h" +#include "NarrowPhaseCollision/Epa.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" +#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h" + +SimplexSolverInterface simplexSolver; +EpaPenetrationDepthSolver epaPenDepthSolver; + +int screenWidth = 640.f; +int screenHeight = 480.f; + +// Scene stuff +SimdPoint3 g_sceneVolumeMin( -1, -1, -1 ); +SimdPoint3 g_sceneVolumeMax( 1, 1, 1 ); + +bool g_shapesPenetrate = false; + +SimdVector3 g_wWitnesses[ 2 ]; + +// Shapes stuff +ConvexShape* g_pConvexShapes[ 2 ] = { 0 }; +SimdTransform g_convexShapesTransform[ 2 ]; + +SimdScalar g_animAngle = SIMD_RADS_PER_DEG; +bool g_pauseAnim = true; + +// 0 - Box ; 1 - Sphere +int g_shapesType[ 2 ] = { 0 }; + +// Box config +SimdVector3 g_boxExtents( 1, 1, 1 ); +// Sphere config +SimdScalar g_sphereRadius = 1; + +float randomFloat( float rangeMin, float rangeMax ) +{ + return ( ( ( float ) ( rand() ) / ( float ) ( RAND_MAX ) ) * ( rangeMax - rangeMin ) + rangeMin ); +} + +int randomShapeType( int minShapeType, int maxShapeType ) +{ + return ( ( ( ( maxShapeType - minShapeType ) + 1 ) * rand() ) / ( ( RAND_MAX + 1 ) + minShapeType ) ); +} + +SimdVector3 randomPosition( const SimdPoint3& minPoint, const SimdPoint3& maxPoint ) +{ + return SimdVector3( randomFloat( minPoint.getX(), maxPoint.getX() ), + randomFloat( minPoint.getY(), maxPoint.getY() ), + randomFloat( minPoint.getZ(), maxPoint.getZ() ) ); +} + +bool createBoxShape( int shapeIndex ) +{ + //#ifdef _DEBUG + //static bool b = true; + // + //if ( b ) + //{ + // g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); + + // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); + + // g_convexShapesTransform[ shapeIndex ].setIdentity(); + + // SimdMatrix3x3 basis( 0.99365157, 0.024418538, -0.10981932, + // -0.025452739, 0.99964380, -0.0080251107, + // 0.10958424, 0.010769366, 0.99391919 ); + + // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 4.4916530, -19.059078, -0.22695254 ) ); + // g_convexShapesTransform[ shapeIndex ].setBasis( basis ); + + // b = false; + //} + //else + //{ + // g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 25, 10, 25 ) ); + + // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); + + // //SimdMatrix3x3 basis( 0.658257, 0.675022, -0.333709, + // // -0.333120, 0.658556, 0.675023, + // // 0.675314, -0.333120, 0.658256 ); + + // g_convexShapesTransform[ shapeIndex ].setIdentity(); + + // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, -30, 0/*0.326090, -0.667531, 0.214331*/ ) ); + // //g_convexShapesTransform[ shapeIndex ].setBasis( basis ); + //} + //#endif + + g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); + + g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 ); + + g_convexShapesTransform[ shapeIndex ].setIdentity(); + + g_convexShapesTransform[ shapeIndex ].setOrigin( randomPosition( g_sceneVolumeMin, g_sceneVolumeMax ) ); + + return true; +} + +bool createSphereShape( int shapeIndex ) +{ + g_pConvexShapes[ shapeIndex ] = new SphereShape( g_sphereRadius ); + + g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 ); + + g_convexShapesTransform[ shapeIndex ].setIdentity(); + g_convexShapesTransform[ shapeIndex ].setOrigin( randomPosition( g_sceneVolumeMin, g_sceneVolumeMax ) ); + + //#ifdef _DEBUG + //static bool b = true; + //if ( b ) + //{ + // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0.001, 0, 0 ) ); + // b = false; + //} + //else + //{ + // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, 0, 0 ) ); + //} + //#endif + + return true; +} + +void destroyShapes() +{ + if ( g_pConvexShapes[ 0 ] ) + { + delete g_pConvexShapes[ 0 ]; + g_pConvexShapes[ 0 ] = 0; + } + + if ( g_pConvexShapes[ 1 ] ) + { + delete g_pConvexShapes[ 1 ]; + g_pConvexShapes[ 1 ] = 0; + } +} + +bool calcPenDepth() +{ + // Ryn Hybrid Pen Depth and EPA if necessary + + SimdVector3 v( 1, 0, 0 ); + + SimdScalar squaredDistance = SIMD_INFINITY; + SimdScalar delta = 0.f; + + const SimdScalar margin = g_pConvexShapes[ 0 ]->GetMargin() + g_pConvexShapes[ 1 ]->GetMargin(); + const SimdScalar marginSqrd = margin * margin; + + SimdScalar maxRelErrorSqrd = 1e-3 * 1e-3; + + simplexSolver.reset(); + + while ( true ) + { + assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" ); + + SimdVector3 seperatingAxisInA = -v * g_convexShapesTransform[ 0 ].getBasis(); + SimdVector3 seperatingAxisInB = v * g_convexShapesTransform[ 1 ].getBasis(); + + SimdVector3 pInA = g_pConvexShapes[ 0 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA ); + SimdVector3 qInB = g_pConvexShapes[ 1 ]->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB ); + + SimdPoint3 pWorld = g_convexShapesTransform[ 0 ]( pInA ); + SimdPoint3 qWorld = g_convexShapesTransform[ 1 ]( qInB ); + + SimdVector3 w = pWorld - qWorld; + delta = v.dot( w ); + + // potential exit, they don't overlap + if ( ( delta > 0 ) && ( ( delta * delta / squaredDistance ) > marginSqrd ) ) + { + // Convex shapes do not overlap + return false; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if ( ( squaredDistance - delta <= squaredDistance * maxRelErrorSqrd ) || simplexSolver.inSimplex( w ) ) + { + simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); + g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); + + return true; + } + + //add current vertex to simplex + simplexSolver.addVertex( w, pWorld, qWorld ); + + //calculate the closest point to the origin (update vector v) + if ( !simplexSolver.closest( v ) ) + { + simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); + g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); + + return true; + } + + SimdScalar previousSquaredDistance = squaredDistance; + squaredDistance = v.length2(); + + //are we getting any closer ? + if ( previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance ) + { + simplexSolver.backup_closest( v ); + squaredDistance = v.length2(); + + simplexSolver.compute_points( g_wWitnesses[ 0 ], g_wWitnesses[ 1 ] ); + + assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" ); + SimdScalar vLength = sqrt( squaredDistance ); + + g_wWitnesses[ 0 ] -= v * ( g_pConvexShapes[ 0 ]->GetMargin() / vLength ); + g_wWitnesses[ 1 ] += v * ( g_pConvexShapes[ 1 ]->GetMargin() / vLength ); + + return true; + } + + if ( simplexSolver.fullSimplex() || ( squaredDistance <= SIMD_EPSILON * simplexSolver.maxVertex() ) ) + { + // Run EPA + break; + } + } + + return epaPenDepthSolver.CalcPenDepth( simplexSolver, g_pConvexShapes[ 0 ], g_pConvexShapes[ 1 ], + g_convexShapesTransform[ 0 ], g_convexShapesTransform[ 1 ], v, + g_wWitnesses[ 0 ], g_wWitnesses[ 1 ], 0 ); +} + +int main(int argc,char** argv) +{ + srand( time( 0 ) ); + + g_shapesType[ 0 ] = randomShapeType( 0, 1 ); + g_shapesType[ 1 ] = randomShapeType( 0, 1 ); + + ( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 ); + ( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 ); + + g_shapesPenetrate = calcPenDepth(); + + return glutmain( argc, argv, screenWidth, screenHeight, "EPAPenDepthDemo" ); +} + +void drawShape( int shapeIndex ) +{ + float m[ 16 ]; + g_convexShapesTransform[ shapeIndex ].getOpenGLMatrix( m ); + + glMultMatrixf( m ); + + if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == BOX_SHAPE_PROXYTYPE ) + { + glutWireCube( ( ( BoxShape* ) g_pConvexShapes[ shapeIndex ] )->GetHalfExtents().x() * 2 ); + } + else if ( g_pConvexShapes[ shapeIndex ]->GetShapeType() == SPHERE_SHAPE_PROXYTYPE ) + { + glutWireSphere( 1, 16, 16 ); + } +} + +void drawPenDepthVector() +{ + glLoadIdentity(); + + glColor3f( 1, 1, 0 ); + + glPointSize( 5 ); + glBegin( GL_POINTS ); + + glVertex3f( g_wWitnesses[ 0 ].getX(), g_wWitnesses[ 0 ].getY(), g_wWitnesses[ 0 ].getZ() ); + glVertex3f( g_wWitnesses[ 1 ].getX(), g_wWitnesses[ 1 ].getY(), g_wWitnesses[ 1 ].getZ() ); + + glEnd(); + + glColor3f( 1, 0, 0 ); + + glBegin( GL_LINES ); + + glVertex3f( g_wWitnesses[ 0 ].getX(), g_wWitnesses[ 0 ].getY(), g_wWitnesses[ 0 ].getZ() ); + glVertex3f( g_wWitnesses[ 1 ].getX(), g_wWitnesses[ 1 ].getY(), g_wWitnesses[ 1 ].getZ() ); + + glEnd(); +} + +void clientMoveAndDisplay() +{ + if ( !g_pauseAnim ) + { + SimdMatrix3x3 rot; + rot.setEulerZYX( g_animAngle * 0.05, g_animAngle * 0.05, g_animAngle * 0.05 ); + + SimdTransform t; + t.setIdentity(); + t.setBasis( rot ); + + //g_convexShapesTransform[ 0 ].mult( g_convexShapesTransform[ 0 ], t ); + g_convexShapesTransform[ 1 ].mult( g_convexShapesTransform[ 1 ], t ); + + g_shapesPenetrate = calcPenDepth(); + } + + clientDisplay(); +} + +void clientDisplay(void) { + + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glDisable( GL_LIGHTING ); + + GL_ShapeDrawer::DrawCoordSystem(); + + glMatrixMode( GL_MODELVIEW ); + + for ( int i = 0; i < 2; ++i ) + { + glPushMatrix(); + + drawShape( i ); + + glPopMatrix(); + } + + if ( g_shapesPenetrate ) + { + glPushMatrix(); + drawPenDepthVector(); + glPopMatrix(); + } + + glFlush(); + glutSwapBuffers(); +} + +void clientResetScene() +{ +} + +void clientKeyboard(unsigned char key, int x, int y) +{ + if ( key == 'R' || key == 'r' ) + { + destroyShapes(); + + g_shapesType[ 0 ] = randomShapeType( 0, 1 ); + g_shapesType[ 1 ] = randomShapeType( 0, 1 ); + + ( g_shapesType[ 0 ] == 0 ) ? createBoxShape( 0 ) : createSphereShape( 0 ); + ( g_shapesType[ 1 ] == 0 ) ? createBoxShape( 1 ) : createSphereShape( 1 ); + + g_shapesPenetrate = calcPenDepth(); + } + else if ( key == 'Q' || key == 'q' ) + { + destroyShapes(); + } + else if ( key == 'T' || key == 't' ) + { +#ifdef DEBUG_ME + SimdVector3 shapeAPos = g_convexShapesTransform[ 0 ].getOrigin(); + SimdVector3 shapeBPos = g_convexShapesTransform[ 1 ].getOrigin(); + + SimdMatrix3x3 shapeARot = g_convexShapesTransform[ 0 ].getBasis(); + SimdMatrix3x3 shapeBRot = g_convexShapesTransform[ 1 ].getBasis(); + + FILE* fp = 0; + + fopen_s( &fp, "shapes.txt", "w" ); + + char str[ 256 ]; + sprintf_s( str, 256, "PosA: %f, %f, %f\nPosB: %f, %f, %f\n", shapeAPos.x(), shapeAPos.y(), shapeAPos.z(), + shapeBPos.x(), shapeBPos.y(), shapeBPos.z() ); + fputs( str, fp ); + + sprintf_s( str, 256, "RotA: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\nRotB: %f, %f, %f\n%f, %f, %f\n%f, %f, %f\n\n", + shapeARot.getRow( 0 ).x(), shapeARot.getRow( 0 ).y(), shapeARot.getRow( 0 ).z(), + shapeARot.getRow( 1 ).x(), shapeARot.getRow( 1 ).y(), shapeARot.getRow( 1 ).z(), + shapeARot.getRow( 2 ).x(), shapeARot.getRow( 2 ).y(), shapeARot.getRow( 2 ).z(), + shapeBRot.getRow( 0 ).x(), shapeBRot.getRow( 0 ).y(), shapeBRot.getRow( 0 ).z(), + shapeBRot.getRow( 1 ).x(), shapeBRot.getRow( 1 ).y(), shapeBRot.getRow( 1 ).z(), + shapeBRot.getRow( 2 ).x(), shapeBRot.getRow( 2 ).y(), shapeBRot.getRow( 2 ).z()); + fputs( str, fp ); + + fclose( fp ); +#endif //DEBUG_ME + } + else if ( key == 'P' || key =='p' ) + { + g_pauseAnim = !g_pauseAnim; + } + + defaultKeyboard(key, x, y); +} + + +void clientMouseFunc(int button, int state, int x, int y) +{ +} + +void clientMotionFunc(int x,int y) +{ +} diff --git a/Demos/EPAPenDepthDemo/Jamfile b/Demos/EPAPenDepthDemo/Jamfile new file mode 100644 index 000000000..a25d75c13 --- /dev/null +++ b/Demos/EPAPenDepthDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos EPAPenDepthDemo ; + +BulletDemo EPAPenDepthDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/GjkConvexCastDemo/Jamfile b/Demos/GjkConvexCastDemo/Jamfile new file mode 100644 index 000000000..3f5f09a92 --- /dev/null +++ b/Demos/GjkConvexCastDemo/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos GjkConvexCastDemo ; + +BulletDemo GjkConvexCastDemo : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp new file mode 100644 index 000000000..23369020f --- /dev/null +++ b/Demos/GjkConvexCastDemo/LinearConvexCastDemo.cpp @@ -0,0 +1,248 @@ +/* + * 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. + */ + + + +/* + LinearConvexCastDemo implements an efficient continuous collision detection algorithm. + Both linear and angular velocities are supported. Gjk or Simplex based methods. + Motion using Exponential Map. + Comparison with Screwing Motion. + Also comparision with Algebraic CCD and Interval Arithmetic methods (Stephane Redon) +*/ + +#include "SimdQuaternion.h" +#include "SimdTransform.h" +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" + +#include "NarrowPhaseCollision/GjkPairDetector.h" +#include "NarrowPhaseCollision/GjkConvexCast.h" +#include "NarrowPhaseCollision/ContinuousConvexCollision.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "NarrowPhaseCollision/BU_CollisionPair.h" + +#include "CollisionShapes/SphereShape.h" + +#include "CollisionShapes/Simplex1to4Shape.h" + +#include "GL_ShapeDrawer.h" +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GlutStuff.h" + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 2; + + +PolyhedralConvexShape* shapePtr[maxNumObjects]; + +SimdTransform tr[numObjects]; +int screenWidth = 640.f; +int screenHeight = 480.f; +void DrawRasterizerLine(float const* , float const*, int) +{ + +} + +int main(int argc,char** argv) +{ + + setCameraDistance(30.f); + tr[0].setOrigin(SimdVector3(0,0,0)); + tr[1].setOrigin(SimdVector3(0,10,0)); + + SimdMatrix3x3 basisA; + basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, + -0.00029313788f,0.99753088f,0.070228584f, + -0.00089153741f,-0.070228823f,0.99753052f); + + SimdMatrix3x3 basisB; + basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, + 4.4865495e-018f,0.97979438f,0.20000751f, + 4.4410586e-017f,-0.20000751f,0.97979438f); + + tr[0].setBasis(basisA); + tr[1].setBasis(basisB); + + + + SimdVector3 boxHalfExtentsA(0.2,4,4); + SimdVector3 boxHalfExtentsB(6,6,6); + + BoxShape boxA(boxHalfExtentsA); +/* BU_Simplex1to4 boxB; + boxB.AddVertex(SimdPoint3(-5,0,-5)); + boxB.AddVertex(SimdPoint3(5,0,-5)); + boxB.AddVertex(SimdPoint3(0,0,5)); + boxB.AddVertex(SimdPoint3(0,5,0)); +*/ + + BoxShape boxB(boxHalfExtentsB); + shapePtr[0] = &boxA; + shapePtr[1] = &boxB; + + shapePtr[0]->SetMargin(0.01f); + shapePtr[1]->SetMargin(0.01f); + +// boxA.SetMargin(1.f); +// boxB.SetMargin(1.f); + + + SimdTransform tr; + tr.setIdentity(); + + + return glutmain(argc, argv,screenWidth,screenHeight,"Linear Convex Cast Demo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h" + +static VoronoiSimplexSolver sVoronoiSimplexSolver; + +SimplexSolverInterface& gGjkSimplexSolver = sVoronoiSimplexSolver; + +bool drawLine= false; + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glDisable(GL_LIGHTING); + + //GL_ShapeDrawer::DrawCoordSystem(); + + float m[16]; + int i; + + for (i=0;i) : noinstall console nomanifest ; + LinkWith $(<) : bullet bulletopenglsupport bulletphysicsinterfacecommon bulletccdphysics bulletmath ; + CFlags $(<) : + [ FIncludes $(TOP)/Demos/OpenGL ] + [ FIncludes $(TOP)/Extras/PhysicsInterface/CcdPhysics ] + [ FIncludes $(TOP)/Extras/PhysicsInterface/Common ] + ; + MsvcIncDirs $(<) : + "../../Demos/OpenGL" + "../../Extras/PhysicsInterface/CcdPhysics" + "../../Extras/PhysicsInterface/Common" ; + } +} +else +{ + rule BulletDemo + { + } +} + + rule BulletBasicDemo + { + Application $(<) : $(>) : noinstall console nomanifest ; + LinkWith $(<) : bullet bulletphysicsinterfacecommon bulletccdphysics bulletmath ; + CFlags $(<) : + [ FIncludes $(TOP)/Extras/PhysicsInterface/CcdPhysics ] + [ FIncludes $(TOP)/Extras/PhysicsInterface/Common ] + ; + MsvcIncDirs $(<) : + "../../Extras/PhysicsInterface/CcdPhysics" + "../../Extras/PhysicsInterface/Common" ; + } + +SubInclude TOP Demos BasicSample ; +SubInclude TOP Demos CcdPhysicsDemo ; +SubInclude TOP Demos CollisionDemo ; +SubInclude TOP Demos CollisionInterfaceDemo ; +SubInclude TOP Demos ConcaveDemo ; +SubInclude TOP Demos ConstraintDemo ; +SubInclude TOP Demos ContinuousConvexCollision ; +SubInclude TOP Demos GjkConvexCastDemo ; +SubInclude TOP Demos EPAPenDepthDemo ; +SubInclude TOP Demos Raytracer ; +SubInclude TOP Demos SimplexDemo ; diff --git a/Demos/OpenGL/BMF_Api.cpp b/Demos/OpenGL/BMF_Api.cpp new file mode 100644 index 000000000..d67191d26 --- /dev/null +++ b/Demos/OpenGL/BMF_Api.cpp @@ -0,0 +1,162 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + * + * Implementation of the API of the OpenGL bitmap font library. + */ + +#include "BMF_Api.h" + +#include "BMF_BitmapFont.h" + + +#if BMF_INCLUDE_HELV10 +extern BMF_FontData BMF_font_helv10; +static BMF_BitmapFont bmfHelv10(&BMF_font_helv10); +#endif // BMF_INCLUDE_HELV10 +#if BMF_INCLUDE_HELV12 +extern BMF_FontData BMF_font_helv12; +static BMF_BitmapFont bmfHelv12(&BMF_font_helv12); +#endif // BMF_INCLUDE_HELV12 +#if BMF_INCLUDE_HELVB8 +extern BMF_FontData BMF_font_helvb8; +static BMF_BitmapFont bmfHelvb8(&BMF_font_helvb8); +#endif // BMF_INCLUDE_HELVB8 +#if BMF_INCLUDE_HELVB10 +extern BMF_FontData BMF_font_helvb10; +static BMF_BitmapFont bmfHelvb10(&BMF_font_helvb10); +#endif // BMF_INCLUDE_HELVB10 +#if BMF_INCLUDE_HELVB12 +extern BMF_FontData BMF_font_helvb12; +static BMF_BitmapFont bmfHelvb12(&BMF_font_helvb12); +#endif // BMF_INCLUDE_HELVB12 +#if BMF_INCLUDE_HELVB14 +extern BMF_FontData BMF_font_helvb14; +static BMF_BitmapFont bmfHelvb14(&BMF_font_helvb14); +#endif // BMF_INCLUDE_HELVB14 +#if BMF_INCLUDE_SCR12 +extern BMF_FontData BMF_font_scr12; +static BMF_BitmapFont bmfScreen12(&BMF_font_scr12); +#endif // BMF_INCLUDE_SCR12 +#if BMF_INCLUDE_SCR14 +extern BMF_FontData BMF_font_scr14; +static BMF_BitmapFont bmfScreen14(&BMF_font_scr14); +#endif // BMF_INCLUDE_SCR14 +#if BMF_INCLUDE_SCR15 +extern BMF_FontData BMF_font_scr15; +static BMF_BitmapFont bmfScreen15(&BMF_font_scr15); +#endif // BMF_INCLUDE_SCR15 + + +BMF_Font* BMF_GetFont(BMF_FontType font) +{ + switch (font) + { +#if BMF_INCLUDE_HELV10 + case BMF_kHelvetica10: return (BMF_Font*) &bmfHelv10; +#endif // BMF_INCLUDE_HELV10 +#if BMF_INCLUDE_HELV12 + case BMF_kHelvetica12: return (BMF_Font*) &bmfHelv12; +#endif // BMF_INCLUDE_HELV12 +#if BMF_INCLUDE_HELVB8 + case BMF_kHelveticaBold8: return (BMF_Font*) &bmfHelvb8; +#endif // BMF_INCLUDE_HELVB8 +#if BMF_INCLUDE_HELVB10 + case BMF_kHelveticaBold10: return (BMF_Font*) &bmfHelvb10; +#endif // BMF_INCLUDE_HELVB10 +#if BMF_INCLUDE_HELVB12 + case BMF_kHelveticaBold12: return (BMF_Font*) &bmfHelvb12; +#endif // BMF_INCLUDE_HELVB12 +#if BMF_INCLUDE_HELVB14 + case BMF_kHelveticaBold14: return (BMF_Font*) &bmfHelvb14; +#endif // BMF_INCLUDE_HELVB12 +#if BMF_INCLUDE_SCR12 + case BMF_kScreen12: return (BMF_Font*) &bmfScreen12; +#endif // BMF_INCLUDE_SCR12 +#if BMF_INCLUDE_SCR14 + case BMF_kScreen14: return (BMF_Font*) &bmfScreen14; +#endif // BMF_INCLUDE_SCR14 +#if BMF_INCLUDE_SCR15 + case BMF_kScreen15: return (BMF_Font*) &bmfScreen15; +#endif // BMF_INCLUDE_SCR15 + default: + break; + } + return 0; +} + + +int BMF_DrawCharacter(BMF_Font* font, char c) +{ + char str[2] = {c, '\0'}; + return BMF_DrawString(font, str); +} + + +int BMF_DrawString(BMF_Font* font, const char* str) +{ + if (!font) return 0; + ((BMF_BitmapFont*)font)->DrawString(str); + return 1; +} + + +int BMF_GetCharacterWidth(BMF_Font* font, char c) +{ + char str[2] = {c, '\0'}; + return BMF_GetStringWidth(font, str); +} + + +int BMF_GetStringWidth(BMF_Font* font, char* str) +{ + if (!font) return 0; + return ((BMF_BitmapFont*)font)->GetStringWidth(str); +} + + +void BMF_GetBoundingBox(BMF_Font* font, int *xmin_r, int *ymin_r, int *xmax_r, int *ymax_r) +{ + if (!font) return; + ((BMF_BitmapFont*)font)->GetBoundingBox(*xmin_r, *ymin_r, *xmax_r, *ymax_r); +} + +int BMF_GetFontTexture(BMF_Font* font) { + if (!font) return -1; + return ((BMF_BitmapFont*)font)->GetTexture(); +} + +void BMF_DrawStringTexture(BMF_Font* font, char *string, float x, float y, float z) { + if (!font) return; + ((BMF_BitmapFont*)font)->DrawStringTexture(string, x, y, z); +} diff --git a/Demos/OpenGL/BMF_Api.h b/Demos/OpenGL/BMF_Api.h new file mode 100644 index 000000000..af23c06ed --- /dev/null +++ b/Demos/OpenGL/BMF_Api.h @@ -0,0 +1,128 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + * + * API of the OpenGL bitmap font library. + * Currently draws fonts using the glBitmap routine. + * This implies that drawing speed is heavyly dependant on + * the 2D capabilities of the graphics card. + */ + +#ifndef __BMF_API_H +#define __BMF_API_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "BMF_Fonts.h" + +/** + * Returns the font for a given font type. + * @param font The font to retrieve. + * @return The font (or nil if not found). + */ +BMF_Font* BMF_GetFont(BMF_FontType font); + +/** + * Draws a character at the current raster position. + * @param font The font to use. + * @param c The character to draw. + * @return Indication of success (0 == error). + */ +int BMF_DrawCharacter(BMF_Font* font, char c); + +/** + * Draws a string at the current raster position. + * @param font The font to use. + * @param str The string to draw. + * @return Indication of success (0 == error). + */ +int BMF_DrawString(BMF_Font* font, const char* str); + +/** + * Returns the width of a character in pixels. + * @param font The font to use. + * @param c The character. + * @return The length. + */ +int BMF_GetCharacterWidth(BMF_Font* font, char c); + +/** + * Returns the width of a string of characters. + * @param font The font to use. + * @param str The string. + * @return The length. + */ +int BMF_GetStringWidth(BMF_Font* font, char* str); + +/** + * Returns the bounding box of the font. The width and + * height represent the bounding box of the union of + * all glyps. The minimum and maximum values of the + * box represent the extent of the font and its positioning + * about the origin. + */ +void BMF_GetBoundingBox(BMF_Font* font, int *xmin_r, int *ymin_r, int *xmax_r, int *ymax_r); + +/** + * Convert the given @a font to a texture, and return the GL texture + * ID of the texture. If the texture ID is bound, text can + * be drawn using the texture by calling DrawStringTexture. + * + * @param font The font to create the texture from. + * @return The GL texture ID of the new texture, or -1 if unable + * to create. + */ +int BMF_GetFontTexture(BMF_Font* font); + +/** + * Draw the given @a str at the point @a x, @a y, @a z, using + * texture coordinates. This assumes that an appropriate texture + * has been bound, see BMF_BitmapFont::GetTexture(). The string + * is drawn along the positive X axis. + * + * @param font The font to draw with. + * @param string The c-string to draw. + * @param x The x coordinate to start drawing at. + * @param y The y coordinate to start drawing at. + * @param z The z coordinate to start drawing at. + */ +void BMF_DrawStringTexture(BMF_Font* font, char* string, float x, float y, float z); + +#ifdef __cplusplus +} +#endif + +#endif /* __BMF_API_H */ + diff --git a/Demos/OpenGL/BMF_BitmapFont.cpp b/Demos/OpenGL/BMF_BitmapFont.cpp new file mode 100644 index 000000000..c42a371e1 --- /dev/null +++ b/Demos/OpenGL/BMF_BitmapFont.cpp @@ -0,0 +1,208 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + */ + +#include + +#if defined(WIN32) || defined(__APPLE__) + #ifdef WIN32 + #if !defined(__CYGWIN32__) + #pragma warning(disable:4244) + #endif /* __CYGWIN32__ */ + #include + #include + #else // WIN32 + // __APPLE__ is defined + #include + #endif // WIN32 +#else // defined(WIN32) || defined(__APPLE__) + #include +#endif // defined(WIN32) || defined(__APPLE__) + +#include "BMF_BitmapFont.h" + + +BMF_BitmapFont::BMF_BitmapFont(BMF_FontData* fontData) +: m_fontData(fontData) +{ +} + + +BMF_BitmapFont::~BMF_BitmapFont(void) +{ +} + + +void BMF_BitmapFont::DrawString(const char* str) +{ + if (!str) + return; + + GLint alignment; + unsigned char c; + + glGetIntegerv(GL_UNPACK_ALIGNMENT, &alignment); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + + while (c = (unsigned char) *str++) { + BMF_CharData & cd = m_fontData->chars[c]; + + if (cd.data_offset==-1) { + GLubyte nullBitmap = 0; + + glBitmap(1, 1, 0, 0, cd.advance, 0, &nullBitmap); + } else { + GLubyte *bitmap = &m_fontData->bitmap_data[cd.data_offset]; + + glBitmap(cd.width, cd.height, cd.xorig, cd.yorig, cd.advance, 0, bitmap); + } + } + + glPixelStorei(GL_UNPACK_ALIGNMENT, alignment); +} + + +int BMF_BitmapFont::GetStringWidth(char* str) +{ + unsigned char c; + int length = 0; + + while (c = (unsigned char) *str++) { + length += m_fontData->chars[c].advance; + } + + return length; +} + +void BMF_BitmapFont::GetBoundingBox(int & xMin, int & yMin, int & xMax, int & yMax) +{ + xMin = m_fontData->xmin; + yMin = m_fontData->ymin; + xMax = m_fontData->xmax; + yMax = m_fontData->ymax; +} + +int BMF_BitmapFont::GetTexture() +{ + int fWidth = m_fontData->xmax - m_fontData->xmin; + int fHeight = m_fontData->ymax - m_fontData->ymin; + + if (fWidth>=16 || fHeight>=16) { + return -1; + } + + int cRows = 16, cCols = 16; + int cWidth = 16, cHeight = 16; + int iWidth = cCols*cWidth; + int iHeight = cRows*cHeight; + GLubyte *img = new GLubyte [iHeight*iWidth]; + GLuint texId; + + int baseLine = -(m_fontData->ymin); + + memset(img, 0, iHeight*iWidth); + for (int i = 0; i<256; i++) { + BMF_CharData & cd = m_fontData->chars[i]; + + if (cd.data_offset != -1) { + int cellX = i%16; + int cellY = i/16; + + for (int y = 0; ybitmap_data[cd.data_offset + ((cd.width+7)/8)*y]; + + for (int x = 0; xymin); + + glBegin(GL_QUADS); + while (c = (unsigned char) *str++) { + BMF_CharData & cd = m_fontData->chars[c]; + + if (cd.data_offset != -1) { + float cellX = (c%16)/16.0; + float cellY = (c/16)/16.0; + + glTexCoord2f(cellX + 1.0/16.0, cellY); + glVertex3f(x + pos + 16.0, -baseLine + y + 0.0, z); + + glTexCoord2f(cellX + 1.0/16.0, cellY + 1.0/16.0); + glVertex3f(x + pos + 16.0, -baseLine + y + 16.0, z); + + glTexCoord2f(cellX, cellY + 1.0/16.0); + glVertex3f(x + pos + 0.0, -baseLine + y + 16.0, z); + + glTexCoord2f(cellX, cellY); + glVertex3f(x + pos + 0.0, -baseLine + y + 0.0, z); + } + + pos += cd.advance; + } + glEnd(); +} diff --git a/Demos/OpenGL/BMF_BitmapFont.h b/Demos/OpenGL/BMF_BitmapFont.h new file mode 100644 index 000000000..4b5bedd14 --- /dev/null +++ b/Demos/OpenGL/BMF_BitmapFont.h @@ -0,0 +1,111 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + */ + +#ifndef __BMF_BITMAP_FONT_H +#define __BMF_BITMAP_FONT_H + +#include "BMF_FontData.h" + +/** + * Base class for OpenGL bitmap fonts. + */ +class BMF_BitmapFont +{ +public: + /** + * Default constructor. + */ + BMF_BitmapFont(BMF_FontData* fontData); + + /** + * Destructor. + */ + virtual ~BMF_BitmapFont(void); + + /** + * Draws a string at the current raster position. + * @param str The string to draw. + */ + void DrawString(const char* str); + + void DrawStringMemory(char* str); + + + /** + * Draws a string at the current raster position. + * @param str The string to draw. + * @return The width of the string. + */ + int GetStringWidth(char* str); + + /** + * Returns the bounding box of the font. The width and + * height represent the bounding box of the union of + * all glyps. The minimum and maximum values of the + * box represent the extent of the font and its positioning + * about the origin. + */ + void GetBoundingBox(int & xMin, int & yMin, int & xMax, int & yMax); + + /** + * Convert the font to a texture, and return the GL texture + * ID of the texture. If the texture ID is bound, text can + * be drawn using the texture by calling DrawStringTexture. + * + * @return The GL texture ID of the new texture, or -1 if unable + * to create. + */ + int GetTexture(); + + /** + * Draw the given @a string at the point @a x, @a y, @a z, using + * texture coordinates. This assumes that an appropriate texture + * has been bound, see BMF_BitmapFont::GetTexture(). The string + * is drawn along the positive X axis. + * + * @param string The c-string to draw. + * @param x The x coordinate to start drawing at. + * @param y The y coordinate to start drawing at. + * @param z The z coordinate to start drawing at. + */ + void DrawStringTexture(char* string, float x, float y, float z); + +protected: + /** Pointer to the font data. */ + BMF_FontData* m_fontData; +}; + +#endif // __BMF_BITMAP_FONT_H + diff --git a/Demos/OpenGL/BMF_FontData.h b/Demos/OpenGL/BMF_FontData.h new file mode 100644 index 000000000..ff827cce7 --- /dev/null +++ b/Demos/OpenGL/BMF_FontData.h @@ -0,0 +1,56 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + */ + +#ifndef __BMF_FONTDATA_H__ +#define __BMF_FONTDATA_H__ + +typedef struct { + signed char width, height; + signed char xorig, yorig; + signed char advance; + + short data_offset; +} BMF_CharData; + +typedef struct { + int xmin, ymin; + int xmax, ymax; + + BMF_CharData chars[256]; + unsigned char* bitmap_data; +} BMF_FontData; + +#endif + diff --git a/Demos/OpenGL/BMF_Fonts.h b/Demos/OpenGL/BMF_Fonts.h new file mode 100644 index 000000000..92c2f3d07 --- /dev/null +++ b/Demos/OpenGL/BMF_Fonts.h @@ -0,0 +1,75 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + * Defines the names of the fonts in the library. + */ + +#ifndef __BMF_FONTS_H +#define __BMF_FONTS_H + +#include "BMF_Settings.h" + +typedef enum +{ + BMF_kHelvetica10 = 0, +#if BMF_INCLUDE_HELV12 + BMF_kHelvetica12, +#endif +#if BMF_INCLUDE_HELVB8 + BMF_kHelveticaBold8, +#endif +#if BMF_INCLUDE_HELVB10 + BMF_kHelveticaBold10, +#endif +#if BMF_INCLUDE_HELVB12 + BMF_kHelveticaBold12, +#endif +#if BMF_INCLUDE_HELVB14 + BMF_kHelveticaBold14, +#endif +#if BMF_INCLUDE_SCR12 + BMF_kScreen12, +#endif +#if BMF_INCLUDE_SCR14 + BMF_kScreen14, +#endif +#if BMF_INCLUDE_SCR15 + BMF_kScreen15, +#endif + BMF_kNumFonts +} BMF_FontType; + +typedef struct BMF_Font BMF_Font; + +#endif /* __BMF_FONTS_H */ + diff --git a/Demos/OpenGL/BMF_Settings.h b/Demos/OpenGL/BMF_Settings.h new file mode 100644 index 000000000..3f38c36ef --- /dev/null +++ b/Demos/OpenGL/BMF_Settings.h @@ -0,0 +1,52 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +/** + + * Copyright (C) 2001 NaN Technologies B.V. + * Allows you to determine which fonts to include in the library. + */ + +#ifndef __BMF_SETTINGS_H +#define __BMF_SETTINGS_H + + +#define BMF_INCLUDE_HELV12 0 +#define BMF_INCLUDE_HELVB8 0 +#define BMF_INCLUDE_HELVB10 0 +#define BMF_INCLUDE_HELVB12 0 +#define BMF_INCLUDE_HELVB14 0 +#define BMF_INCLUDE_SCR12 0 +#define BMF_INCLUDE_SCR14 0 +#define BMF_INCLUDE_SCR15 0 +#define BMF_INCLUDE_HELV10 1 + +#endif /* __BMF_SETTINGS_H */ + diff --git a/Demos/OpenGL/BMF_font_helv10.cpp b/Demos/OpenGL/BMF_font_helv10.cpp new file mode 100644 index 000000000..05252c89c --- /dev/null +++ b/Demos/OpenGL/BMF_font_helv10.cpp @@ -0,0 +1,491 @@ +/** + * ***** BEGIN GPL/BL DUAL LICENSE BLOCK ***** + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. The Blender + * Foundation also sells licenses for use in proprietary software under + * the Blender License. See http://www.blender.org/BL/ for information + * about this. + * + * This program 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software Foundation, + * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. + * All rights reserved. + * + * The Original Code is: all of this file. + * + * Contributor(s): none yet. + * + * ***** END GPL/BL DUAL LICENSE BLOCK ***** + */ + +#include "BMF_FontData.h" +#include "BMF_Settings.h" + +#if BMF_INCLUDE_HELV10 + +static unsigned char bitmap_data[]= { + 0x80,0x00,0x80,0x80,0x80,0x80,0x80,0x80, + 0xa0,0xa0,0x50,0x50,0xf8,0x28,0x7c,0x28, + 0x28,0x20,0x70,0xa8,0x28,0x70,0xa0,0xa8, + 0x70,0x20,0x26,0x29,0x16,0x10,0x08,0x68, + 0x94,0x64,0x64,0x98,0x98,0xa4,0x60,0x50, + 0x50,0x20,0x80,0x40,0x40,0x20,0x40,0x40, + 0x80,0x80,0x80,0x80,0x40,0x40,0x20,0x80, + 0x40,0x40,0x20,0x20,0x20,0x20,0x40,0x40, + 0x80,0xa0,0x40,0xa0,0x20,0x20,0xf8,0x20, + 0x20,0x80,0x40,0x40,0xf8,0x80,0x80,0x80, + 0x40,0x40,0x40,0x40,0x20,0x20,0x70,0x88, + 0x88,0x88,0x88,0x88,0x88,0x70,0x40,0x40, + 0x40,0x40,0x40,0x40,0xc0,0x40,0xf8,0x80, + 0x40,0x30,0x08,0x08,0x88,0x70,0x70,0x88, + 0x08,0x08,0x30,0x08,0x88,0x70,0x10,0x10, + 0xf8,0x90,0x50,0x50,0x30,0x10,0x70,0x88, + 0x08,0x08,0xf0,0x80,0x80,0xf8,0x70,0x88, + 0x88,0xc8,0xb0,0x80,0x88,0x70,0x40,0x40, + 0x20,0x20,0x10,0x10,0x08,0xf8,0x70,0x88, + 0x88,0x88,0x70,0x88,0x88,0x70,0x70,0x88, + 0x08,0x68,0x98,0x88,0x88,0x70,0x80,0x00, + 0x00,0x00,0x00,0x80,0x80,0x40,0x40,0x00, + 0x00,0x00,0x00,0x40,0x20,0x40,0x80,0x40, + 0x20,0xf0,0x00,0xf0,0x80,0x40,0x20,0x40, + 0x80,0x40,0x00,0x40,0x40,0x20,0x10,0x90, + 0x60,0x3e,0x00,0x40,0x00,0x9b,0x00,0xa4, + 0x80,0xa4,0x80,0xa2,0x40,0x92,0x40,0x4d, + 0x40,0x20,0x80,0x1f,0x00,0x82,0x82,0x7c, + 0x44,0x28,0x28,0x10,0x10,0xf0,0x88,0x88, + 0x88,0xf0,0x88,0x88,0xf0,0x78,0x84,0x80, + 0x80,0x80,0x80,0x84,0x78,0xf0,0x88,0x84, + 0x84,0x84,0x84,0x88,0xf0,0xf8,0x80,0x80, + 0x80,0xf8,0x80,0x80,0xf8,0x80,0x80,0x80, + 0x80,0xf0,0x80,0x80,0xf8,0x74,0x8c,0x84, + 0x8c,0x80,0x80,0x84,0x78,0x84,0x84,0x84, + 0x84,0xfc,0x84,0x84,0x84,0x80,0x80,0x80, + 0x80,0x80,0x80,0x80,0x80,0x60,0x90,0x10, + 0x10,0x10,0x10,0x10,0x10,0x88,0x88,0x90, + 0x90,0xe0,0xa0,0x90,0x88,0xf0,0x80,0x80, + 0x80,0x80,0x80,0x80,0x80,0x92,0x92,0x92, + 0xaa,0xaa,0xc6,0xc6,0x82,0x8c,0x8c,0x94, + 0x94,0xa4,0xa4,0xc4,0xc4,0x78,0x84,0x84, + 0x84,0x84,0x84,0x84,0x78,0x80,0x80,0x80, + 0x80,0xf0,0x88,0x88,0xf0,0x02,0x7c,0x8c, + 0x94,0x84,0x84,0x84,0x84,0x78,0x88,0x88, + 0x88,0x88,0xf0,0x88,0x88,0xf0,0x70,0x88, + 0x88,0x08,0x70,0x80,0x88,0x70,0x20,0x20, + 0x20,0x20,0x20,0x20,0x20,0xf8,0x78,0x84, + 0x84,0x84,0x84,0x84,0x84,0x84,0x10,0x28, + 0x28,0x44,0x44,0x44,0x82,0x82,0x22,0x00, + 0x22,0x00,0x22,0x00,0x55,0x00,0x49,0x00, + 0x49,0x00,0x88,0x80,0x88,0x80,0x88,0x88, + 0x50,0x50,0x20,0x50,0x88,0x88,0x10,0x10, + 0x10,0x28,0x28,0x44,0x44,0x82,0xf8,0x80, + 0x40,0x20,0x20,0x10,0x08,0xf8,0xc0,0x80, + 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0xc0, + 0x20,0x20,0x40,0x40,0x40,0x40,0x80,0x80, + 0xc0,0x40,0x40,0x40,0x40,0x40,0x40,0x40, + 0x40,0xc0,0x88,0x50,0x50,0x20,0x20,0xfc, + 0x80,0x80,0x40,0x68,0x90,0x90,0x70,0x10, + 0xe0,0xb0,0xc8,0x88,0x88,0xc8,0xb0,0x80, + 0x80,0x60,0x90,0x80,0x80,0x90,0x60,0x68, + 0x98,0x88,0x88,0x98,0x68,0x08,0x08,0x60, + 0x90,0x80,0xf0,0x90,0x60,0x40,0x40,0x40, + 0x40,0x40,0xe0,0x40,0x30,0x70,0x08,0x68, + 0x98,0x88,0x88,0x98,0x68,0x88,0x88,0x88, + 0x88,0xc8,0xb0,0x80,0x80,0x80,0x80,0x80, + 0x80,0x80,0x80,0x00,0x80,0x80,0x80,0x80, + 0x80,0x80,0x80,0x80,0x00,0x80,0x90,0x90, + 0xa0,0xc0,0xa0,0x90,0x80,0x80,0x80,0x80, + 0x80,0x80,0x80,0x80,0x80,0x80,0x92,0x92, + 0x92,0x92,0x92,0xec,0x88,0x88,0x88,0x88, + 0xc8,0xb0,0x70,0x88,0x88,0x88,0x88,0x70, + 0x80,0x80,0xb0,0xc8,0x88,0x88,0xc8,0xb0, + 0x08,0x08,0x68,0x98,0x88,0x88,0x98,0x68, + 0x80,0x80,0x80,0x80,0xc0,0xa0,0x60,0x90, + 0x10,0x60,0x90,0x60,0x60,0x40,0x40,0x40, + 0x40,0xe0,0x40,0x40,0x70,0x90,0x90,0x90, + 0x90,0x90,0x20,0x20,0x50,0x50,0x88,0x88, + 0x28,0x28,0x54,0x54,0x92,0x92,0x88,0x88, + 0x50,0x20,0x50,0x88,0x80,0x40,0x40,0x60, + 0xa0,0xa0,0x90,0x90,0xf0,0x80,0x40,0x20, + 0x10,0xf0,0x20,0x40,0x40,0x40,0x40,0x80, + 0x40,0x40,0x40,0x20,0x80,0x80,0x80,0x80, + 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x40, + 0x40,0x40,0x40,0x20,0x40,0x40,0x40,0x80, + 0x98,0x64,0x80,0x80,0x80,0x80,0x80,0x80, + 0x00,0x80,0x40,0x70,0xa8,0xa0,0xa0,0xa8, + 0x70,0x10,0xb0,0x48,0x40,0x40,0xe0,0x40, + 0x48,0x30,0x90,0x60,0x90,0x90,0x60,0x90, + 0x20,0xf8,0x20,0xf8,0x50,0x50,0x88,0x88, + 0x80,0x80,0x80,0x80,0x00,0x00,0x80,0x80, + 0x80,0x80,0x70,0x88,0x18,0x70,0xc8,0x98, + 0x70,0xc0,0x88,0x70,0xa0,0x38,0x44,0x9a, + 0xa2,0x9a,0x44,0x38,0xe0,0x00,0xa0,0x20, + 0xe0,0x28,0x50,0xa0,0x50,0x28,0x08,0x08, + 0xf8,0xe0,0x38,0x44,0xaa,0xb2,0xba,0x44, + 0x38,0xe0,0x60,0x90,0x90,0x60,0xf8,0x00, + 0x20,0x20,0xf8,0x20,0x20,0xe0,0x40,0xa0, + 0x60,0xc0,0x20,0x40,0xe0,0x80,0x40,0x80, + 0x80,0xf0,0x90,0x90,0x90,0x90,0x90,0x28, + 0x28,0x28,0x28,0x28,0x68,0xe8,0xe8,0xe8, + 0x7c,0xc0,0xc0,0x40,0x40,0x40,0xc0,0x40, + 0xe0,0x00,0xe0,0xa0,0xe0,0xa0,0x50,0x28, + 0x50,0xa0,0x21,0x00,0x17,0x80,0x13,0x00, + 0x09,0x00,0x48,0x00,0x44,0x00,0xc4,0x00, + 0x42,0x00,0x27,0x12,0x15,0x0b,0x48,0x44, + 0xc4,0x42,0x21,0x00,0x17,0x80,0x13,0x00, + 0x09,0x00,0xc8,0x00,0x24,0x00,0x44,0x00, + 0xe2,0x00,0x60,0x90,0x80,0x40,0x20,0x20, + 0x00,0x20,0x82,0x82,0x7c,0x44,0x28,0x28, + 0x10,0x10,0x00,0x10,0x20,0x82,0x82,0x7c, + 0x44,0x28,0x28,0x10,0x10,0x00,0x10,0x08, + 0x82,0x82,0x7c,0x44,0x28,0x28,0x10,0x10, + 0x00,0x28,0x10,0x82,0x82,0x7c,0x44,0x28, + 0x28,0x10,0x10,0x00,0x28,0x14,0x82,0x82, + 0x7c,0x44,0x28,0x28,0x10,0x10,0x00,0x28, + 0x82,0x82,0x7c,0x44,0x28,0x28,0x10,0x10, + 0x10,0x28,0x10,0x8f,0x80,0x88,0x00,0x78, + 0x00,0x48,0x00,0x2f,0x80,0x28,0x00,0x18, + 0x00,0x1f,0x80,0x30,0x10,0x78,0x84,0x80, + 0x80,0x80,0x80,0x84,0x78,0xf8,0x80,0x80, + 0x80,0xf8,0x80,0x80,0xf8,0x00,0x20,0x40, + 0xf8,0x80,0x80,0x80,0xf8,0x80,0x80,0xf8, + 0x00,0x20,0x10,0xf8,0x80,0x80,0xf8,0x80, + 0x80,0x80,0xf8,0x00,0x50,0x20,0xf8,0x80, + 0x80,0x80,0xf8,0x80,0x80,0xf8,0x00,0x50, + 0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40, + 0x00,0x40,0x80,0x80,0x80,0x80,0x80,0x80, + 0x80,0x80,0x80,0x00,0x80,0x40,0x40,0x40, + 0x40,0x40,0x40,0x40,0x40,0x40,0x00,0xa0, + 0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x40, + 0x40,0x00,0xa0,0x78,0x44,0x42,0x42,0xf2, + 0x42,0x44,0x78,0x8c,0x8c,0x94,0x94,0xa4, + 0xa4,0xc4,0xc4,0x00,0x50,0x28,0x78,0x84, + 0x84,0x84,0x84,0x84,0x84,0x78,0x00,0x10, + 0x20,0x78,0x84,0x84,0x84,0x84,0x84,0x84, + 0x78,0x00,0x10,0x08,0x78,0x84,0x84,0x84, + 0x84,0x84,0x84,0x78,0x00,0x28,0x10,0x78, + 0x84,0x84,0x84,0x84,0x84,0x84,0x78,0x00, + 0x50,0x28,0x78,0x84,0x84,0x84,0x84,0x84, + 0x84,0x78,0x00,0x48,0x88,0x50,0x20,0x50, + 0x88,0x80,0x78,0xc4,0xa4,0xa4,0x94,0x94, + 0x8c,0x78,0x04,0x78,0x84,0x84,0x84,0x84, + 0x84,0x84,0x84,0x00,0x10,0x20,0x78,0x84, + 0x84,0x84,0x84,0x84,0x84,0x84,0x00,0x20, + 0x10,0x78,0x84,0x84,0x84,0x84,0x84,0x84, + 0x84,0x00,0x28,0x10,0x78,0x84,0x84,0x84, + 0x84,0x84,0x84,0x84,0x00,0x48,0x10,0x10, + 0x10,0x28,0x28,0x44,0x44,0x82,0x00,0x10, + 0x08,0x80,0x80,0xf0,0x88,0x88,0xf0,0x80, + 0x80,0xa0,0x90,0x90,0x90,0xa0,0x90,0x90, + 0x60,0x68,0x90,0x90,0x70,0x10,0xe0,0x00, + 0x20,0x40,0x68,0x90,0x90,0x70,0x10,0xe0, + 0x00,0x20,0x10,0x68,0x90,0x90,0x70,0x10, + 0xe0,0x00,0x50,0x20,0x68,0x90,0x90,0x70, + 0x10,0xe0,0x00,0xa0,0x50,0x68,0x90,0x90, + 0x70,0x10,0xe0,0x00,0x50,0x68,0x90,0x90, + 0x70,0x10,0xe0,0x20,0x50,0x20,0x6c,0x92, + 0x90,0x7e,0x12,0xec,0x60,0x20,0x60,0x90, + 0x80,0x80,0x90,0x60,0x60,0x90,0x80,0xf0, + 0x90,0x60,0x00,0x20,0x40,0x60,0x90,0x80, + 0xf0,0x90,0x60,0x00,0x40,0x20,0x60,0x90, + 0x80,0xf0,0x90,0x60,0x00,0x50,0x20,0x60, + 0x90,0x80,0xf0,0x90,0x60,0x00,0x50,0x40, + 0x40,0x40,0x40,0x40,0x40,0x00,0x40,0x80, + 0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x80, + 0x40,0x40,0x40,0x40,0x40,0x40,0x40,0x00, + 0xa0,0x40,0x40,0x40,0x40,0x40,0x40,0x40, + 0x00,0xa0,0x70,0x88,0x88,0x88,0x88,0x78, + 0x90,0x60,0x50,0x90,0x90,0x90,0x90,0x90, + 0xe0,0x00,0xa0,0x50,0x70,0x88,0x88,0x88, + 0x88,0x70,0x00,0x20,0x40,0x70,0x88,0x88, + 0x88,0x88,0x70,0x00,0x20,0x10,0x70,0x88, + 0x88,0x88,0x88,0x70,0x00,0x50,0x20,0x70, + 0x88,0x88,0x88,0x88,0x70,0x00,0x50,0x28, + 0x70,0x88,0x88,0x88,0x88,0x70,0x00,0x50, + 0x20,0x00,0xf8,0x00,0x20,0x70,0x88,0xc8, + 0xa8,0x98,0x74,0x70,0x90,0x90,0x90,0x90, + 0x90,0x00,0x20,0x40,0x70,0x90,0x90,0x90, + 0x90,0x90,0x00,0x40,0x20,0x70,0x90,0x90, + 0x90,0x90,0x90,0x00,0x50,0x20,0x70,0x90, + 0x90,0x90,0x90,0x90,0x00,0x50,0x80,0x40, + 0x40,0x60,0xa0,0xa0,0x90,0x90,0x00,0x20, + 0x10,0x80,0x80,0xb0,0xc8,0x88,0x88,0xc8, + 0xb0,0x80,0x80,0x80,0x40,0x40,0x60,0xa0, + 0xa0,0x90,0x90,0x00,0x50, +}; + +BMF_FontData BMF_font_helv10 = { + -1, -2, + 10, 11, + { + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0, 0, 0, 0, 12, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0, 0, 0, 0, 3, -1}, + {1, 8, -1, 0, 3, 0}, + {3, 2, -1, -6, 4, 8}, + {6, 7, 0, 0, 6, 10}, + {5, 9, 0, 1, 6, 17}, + {8, 8, 0, 0, 9, 26}, + {6, 8, -1, 0, 8, 34}, + {2, 3, -1, -5, 3, 42}, + {3, 10, 0, 2, 4, 45}, + {3, 10, -1, 2, 4, 55}, + {3, 3, 0, -5, 4, 65}, + {5, 5, 0, -1, 6, 68}, + {2, 3, 0, 2, 3, 73}, + {5, 1, -1, -3, 7, 76}, + {1, 1, -1, 0, 3, 77}, + {3, 8, 0, 0, 3, 78}, + {5, 8, 0, 0, 6, 86}, + {2, 8, -1, 0, 6, 94}, + {5, 8, 0, 0, 6, 102}, + {5, 8, 0, 0, 6, 110}, + {5, 8, 0, 0, 6, 118}, + {5, 8, 0, 0, 6, 126}, + {5, 8, 0, 0, 6, 134}, + {5, 8, 0, 0, 6, 142}, + {5, 8, 0, 0, 6, 150}, + {5, 8, 0, 0, 6, 158}, + {1, 6, -1, 0, 3, 166}, + {2, 8, 0, 2, 3, 172}, + {3, 5, -1, -1, 6, 180}, + {4, 3, 0, -2, 5, 185}, + {3, 5, -1, -1, 6, 188}, + {4, 8, -1, 0, 6, 193}, + {10, 10, 0, 2, 11, 201}, + {7, 8, 0, 0, 7, 221}, + {5, 8, -1, 0, 7, 229}, + {6, 8, -1, 0, 8, 237}, + {6, 8, -1, 0, 8, 245}, + {5, 8, -1, 0, 7, 253}, + {5, 8, -1, 0, 6, 261}, + {6, 8, -1, 0, 8, 269}, + {6, 8, -1, 0, 8, 277}, + {1, 8, -1, 0, 3, 285}, + {4, 8, 0, 0, 5, 293}, + {5, 8, -1, 0, 7, 301}, + {4, 8, -1, 0, 6, 309}, + {7, 8, -1, 0, 9, 317}, + {6, 8, -1, 0, 8, 325}, + {6, 8, -1, 0, 8, 333}, + {5, 8, -1, 0, 7, 341}, + {7, 9, -1, 1, 8, 349}, + {5, 8, -1, 0, 7, 358}, + {5, 8, -1, 0, 7, 366}, + {5, 8, 0, 0, 5, 374}, + {6, 8, -1, 0, 8, 382}, + {7, 8, 0, 0, 7, 390}, + {9, 8, 0, 0, 9, 398}, + {5, 8, -1, 0, 7, 414}, + {7, 8, 0, 0, 7, 422}, + {5, 8, -1, 0, 7, 430}, + {2, 10, -1, 2, 3, 438}, + {3, 8, 0, 0, 3, 448}, + {2, 10, 0, 2, 3, 456}, + {5, 5, 0, -3, 6, 466}, + {6, 1, 0, 2, 6, 471}, + {2, 3, 0, -5, 3, 472}, + {5, 6, 0, 0, 5, 475}, + {5, 8, 0, 0, 6, 481}, + {4, 6, 0, 0, 5, 489}, + {5, 8, 0, 0, 6, 495}, + {4, 6, 0, 0, 5, 503}, + {4, 8, 0, 0, 4, 509}, + {5, 8, 0, 2, 6, 517}, + {5, 8, 0, 0, 6, 525}, + {1, 8, 0, 0, 2, 533}, + {1, 9, 0, 1, 2, 541}, + {4, 8, 0, 0, 5, 550}, + {1, 8, 0, 0, 2, 558}, + {7, 6, 0, 0, 8, 566}, + {5, 6, 0, 0, 6, 572}, + {5, 6, 0, 0, 6, 578}, + {5, 8, 0, 2, 6, 584}, + {5, 8, 0, 2, 6, 592}, + {3, 6, 0, 0, 4, 600}, + {4, 6, 0, 0, 5, 606}, + {3, 8, 0, 0, 4, 612}, + {4, 6, 0, 0, 5, 620}, + {5, 6, 0, 0, 6, 626}, + {7, 6, 0, 0, 8, 632}, + {5, 6, 0, 0, 6, 638}, + {4, 8, 0, 2, 5, 644}, + {4, 6, 0, 0, 5, 652}, + {3, 10, 0, 2, 3, 658}, + {1, 10, -1, 2, 3, 668}, + {3, 10, 0, 2, 3, 678}, + {6, 2, 0, -3, 7, 688}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0,0,0,0,0, -1}, + {0, 0, 0, 0, 3, -1}, + {1, 8, -1, 2, 3, 690}, + {5, 8, 0, 1, 6, 698}, + {5, 8, 0, 0, 6, 706}, + {4, 6, 0, -1, 5, 714}, + {5, 8, 0, 0, 6, 720}, + {1, 10, -1, 2, 3, 728}, + {5, 10, 0, 2, 6, 738}, + {3, 1, 0, -7, 3, 748}, + {7, 7, -1, 0, 9, 749}, + {3, 5, 0, -3, 4, 756}, + {5, 5, 0, 0, 6, 761}, + {5, 3, -1, -2, 7, 766}, + {3, 1, 0, -3, 4, 769}, + {7, 7, -1, 0, 9, 770}, + {3, 1, 0, -7, 3, 777}, + {4, 4, 0, -3, 4, 778}, + {5, 7, 0, 0, 6, 782}, + {3, 4, 0, -3, 3, 789}, + {3, 4, 0, -3, 3, 793}, + {2, 2, 0, -6, 3, 797}, + {4, 8, 0, 2, 5, 799}, + {6, 10, 0, 2, 6, 807}, + {2, 1, 0, -3, 3, 817}, + {2, 2, 0, 2, 3, 818}, + {2, 4, 0, -3, 3, 820}, + {3, 5, 0, -3, 4, 824}, + {5, 5, 0, 0, 6, 829}, + {9, 8, 0, 0, 9, 834}, + {8, 8, 0, 0, 9, 850}, + {9, 8, 0, 0, 9, 858}, + {4, 8, -1, 2, 6, 874}, + {7, 11, 0, 0, 7, 882}, + {7, 11, 0, 0, 7, 893}, + {7, 11, 0, 0, 7, 904}, + {7, 11, 0, 0, 7, 915}, + {7, 10, 0, 0, 7, 926}, + {7, 11, 0, 0, 7, 936}, + {9, 8, 0, 0, 10, 947}, + {6, 10, -1, 2, 8, 963}, + {5, 11, -1, 0, 7, 973}, + {5, 11, -1, 0, 7, 984}, + {5, 11, -1, 0, 7, 995}, + {5, 10, -1, 0, 7, 1006}, + {2, 11, 0, 0, 3, 1016}, + {2, 11, -1, 0, 3, 1027}, + {3, 11, 0, 0, 3, 1038}, + {3, 10, 0, 0, 3, 1049}, + {7, 8, 0, 0, 8, 1059}, + {6, 11, -1, 0, 8, 1067}, + {6, 11, -1, 0, 8, 1078}, + {6, 11, -1, 0, 8, 1089}, + {6, 11, -1, 0, 8, 1100}, + {6, 11, -1, 0, 8, 1111}, + {6, 10, -1, 0, 8, 1122}, + {5, 5, 0, -1, 6, 1132}, + {6, 10, -1, 1, 8, 1137}, + {6, 11, -1, 0, 8, 1147}, + {6, 11, -1, 0, 8, 1158}, + {6, 11, -1, 0, 8, 1169}, + {6, 10, -1, 0, 8, 1180}, + {7, 11, 0, 0, 7, 1190}, + {5, 8, -1, 0, 7, 1201}, + {4, 8, 0, 0, 5, 1209}, + {5, 9, 0, 0, 5, 1217}, + {5, 9, 0, 0, 5, 1226}, + {5, 9, 0, 0, 5, 1235}, + {5, 9, 0, 0, 5, 1244}, + {5, 8, 0, 0, 5, 1253}, + {5, 9, 0, 0, 5, 1261}, + {7, 6, 0, 0, 8, 1270}, + {4, 8, 0, 2, 5, 1276}, + {4, 9, 0, 0, 5, 1284}, + {4, 9, 0, 0, 5, 1293}, + {4, 9, 0, 0, 5, 1302}, + {4, 8, 0, 0, 5, 1311}, + {2, 9, 1, 0, 2, 1319}, + {2, 9, 0, 0, 2, 1328}, + {3, 9, 1, 0, 2, 1337}, + {3, 8, 0, 0, 2, 1346}, + {5, 9, 0, 0, 6, 1354}, + {4, 9, 0, 0, 5, 1363}, + {5, 9, 0, 0, 6, 1372}, + {5, 9, 0, 0, 6, 1381}, + {5, 9, 0, 0, 6, 1390}, + {5, 9, 0, 0, 6, 1399}, + {5, 8, 0, 0, 6, 1408}, + {5, 5, 0, -1, 6, 1416}, + {6, 6, 0, 0, 6, 1421}, + {4, 9, 0, 0, 5, 1427}, + {4, 9, 0, 0, 5, 1436}, + {4, 9, 0, 0, 5, 1445}, + {4, 8, 0, 0, 5, 1454}, + {4, 11, 0, 2, 5, 1462}, + {5, 10, 0, 2, 6, 1473}, + {4, 10, 0, 2, 5, 1483}, + }, + bitmap_data +}; + +#endif + diff --git a/Demos/OpenGL/DebugCastResult.h b/Demos/OpenGL/DebugCastResult.h new file mode 100644 index 000000000..17bc19f13 --- /dev/null +++ b/Demos/OpenGL/DebugCastResult.h @@ -0,0 +1,76 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 DEBUG_CAST_RESULT_H +#define DEBUG_CAST_RESULT_H + +#include "NarrowPhaseCollision/ConvexCast.h" +#include "SimdTransform.h" +#include "GL_ShapeDrawer.h" +#ifdef WIN32 +#include +#endif +#include +struct DebugCastResult : public ConvexCast::CastResult +{ + + SimdTransform m_fromTrans; + const PolyhedralConvexShape* m_shape; + SimdVector3 m_linVel; + SimdVector3 m_angVel; + + DebugCastResult(const SimdTransform& fromTrans,const PolyhedralConvexShape* shape, + const SimdVector3& linVel,const SimdVector3& angVel) + :m_fromTrans(fromTrans), + m_shape(shape), + m_linVel(linVel), + m_angVel(angVel) + { + } + + virtual void DrawCoordSystem(const SimdTransform& tr) + { + float m[16]; + tr.getOpenGLMatrix(m); + glPushMatrix(); + glLoadMatrixf(m); + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(0, 0, 0); + glVertex3d(1, 0, 0); + glColor3f(0, 1, 0); + glVertex3d(0, 0, 0); + glVertex3d(0, 1, 0); + glColor3f(0, 0, 1); + glVertex3d(0, 0, 0); + glVertex3d(0, 0, 1); + glEnd(); + glPopMatrix(); + } + + virtual void DebugDraw(SimdScalar fraction) + { + + float m[16]; + SimdTransform hitTrans; + SimdTransformUtil::IntegrateTransform(m_fromTrans,m_linVel,m_angVel,fraction,hitTrans); + hitTrans.getOpenGLMatrix(m); + GL_ShapeDrawer::DrawOpenGL(m,m_shape,SimdVector3(1,0,0),IDebugDraw::DBG_NoDebug); + + } +}; + + +#endif //DEBUG_CAST_RESULT_H diff --git a/Demos/OpenGL/GLDebugDrawer.cpp b/Demos/OpenGL/GLDebugDrawer.cpp new file mode 100644 index 000000000..8d3bdbfeb --- /dev/null +++ b/Demos/OpenGL/GLDebugDrawer.cpp @@ -0,0 +1,53 @@ + +#include "GLDebugDrawer.h" +#include "SimdPoint3.h" + +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "BMF_Api.h" +#include //printf debugging +GLDebugDrawer::GLDebugDrawer() +:m_debugMode(0) +{ + +} +void GLDebugDrawer::DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color) +{ + if (m_debugMode > 0) + { + glBegin(GL_LINES); + glColor3f(color.getX(), color.getY(), color.getZ()); + glVertex3d(from.getX(), from.getY(), from.getZ()); + glVertex3d(to.getX(), to.getY(), to.getZ()); + glEnd(); + } +} + +void GLDebugDrawer::SetDebugMode(int debugMode) +{ + m_debugMode = debugMode; + +} + +void GLDebugDrawer::DrawContactPoint(const SimdVector3& pointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color) +{ + if (m_debugMode & IDebugDraw::DBG_DrawContactPoints) + { + SimdVector3 to=pointOnB+normalOnB*distance; + const SimdVector3&from = pointOnB; + glBegin(GL_LINES); + glColor3f(color.getX(), color.getY(), color.getZ()); + glVertex3d(from.getX(), from.getY(), from.getZ()); + glVertex3d(to.getX(), to.getY(), to.getZ()); + glEnd(); + + glRasterPos3f(from.x(), from.y(), from.z()); + char buf[12]; + sprintf(buf," %d",lifeTime); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + + } +} diff --git a/Demos/OpenGL/GLDebugDrawer.h b/Demos/OpenGL/GLDebugDrawer.h new file mode 100644 index 000000000..4b20c757d --- /dev/null +++ b/Demos/OpenGL/GLDebugDrawer.h @@ -0,0 +1,26 @@ +#ifndef GL_DEBUG_DRAWER_H +#define GL_DEBUG_DRAWER_H + +#include "IDebugDraw.h" + + + +class GLDebugDrawer : public IDebugDraw +{ + int m_debugMode; + +public: + + GLDebugDrawer(); + + virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color); + + virtual void DrawContactPoint(const SimdVector3& PointOnB,const SimdVector3& normalOnB,float distance,int lifeTime,const SimdVector3& color); + + virtual void SetDebugMode(int debugMode); + + virtual int GetDebugMode() const { return m_debugMode;} + +}; + +#endif//GL_DEBUG_DRAWER_H diff --git a/Demos/OpenGL/GL_ShapeDrawer.cpp b/Demos/OpenGL/GL_ShapeDrawer.cpp new file mode 100644 index 000000000..9c316b5c6 --- /dev/null +++ b/Demos/OpenGL/GL_ShapeDrawer.cpp @@ -0,0 +1,232 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GL_ShapeDrawer.h" +#include "CollisionShapes/PolyhedralConvexShape.h" +#include "CollisionShapes/TriangleMeshShape.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/SphereShape.h" +#include "CollisionShapes/ConeShape.h" +#include "CollisionShapes/CylinderShape.h" +#include "CollisionShapes/Simplex1to4Shape.h" + + + + +#include "IDebugDraw.h" +//for debugmodes +#include "BMF_Api.h" +#include //printf debugging + +void GL_ShapeDrawer::DrawCoordSystem() { + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(0, 0, 0); + glVertex3d(1, 0, 0); + glColor3f(0, 1, 0); + glVertex3d(0, 0, 0); + glVertex3d(0, 1, 0); + glColor3f(0, 0, 1); + glVertex3d(0, 0, 0); + glVertex3d(0, 0, 1); + glEnd(); + +} + + + + +class GlDrawcallback : public TriangleCallback +{ +public: + + virtual void ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex) + { + glBegin(GL_LINES); + glColor3f(1, 0, 0); + glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ()); + glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ()); + glColor3f(0, 1, 0); + glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ()); + glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ()); + glColor3f(0, 0, 1); + glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ()); + glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ()); + glEnd(); + + } + +}; + +void GL_ShapeDrawer::DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int debugMode) +{ + glPushMatrix(); + glLoadMatrixf(m); + + //DrawCoordSystem(); + + glPushMatrix(); + glEnable(GL_COLOR_MATERIAL); + glColor3f(color.x(),color.y(), color.z()); + + glRasterPos3f(0.0, 0.0, 0.0); + + bool useWireframeFallback = true; + + if (!(debugMode & IDebugDraw::DBG_DrawWireframe)) + { + switch (shape->GetShapeType()) + { + case BOX_SHAPE_PROXYTYPE: + { + const BoxShape* boxShape = static_cast(shape); + SimdVector3 halfExtent = boxShape->GetHalfExtents(); + glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]); + glutSolidCube(1.0); + useWireframeFallback = false; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + case TETRAHEDRAL_SHAPE_PROXYTYPE: + { + const BU_Simplex1to4* tetra = static_cast(shape); + //todo: + useWireframeFallback = false; + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + case SPHERE_SHAPE_PROXYTYPE: + { + const SphereShape* sphereShape = static_cast(shape); + float radius = sphereShape->GetMargin();//radius doesn't include the margin, so draw with margin + glutSolidSphere(radius,10,10); + useWireframeFallback = false; + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + case CONE_SHAPE_PROXYTYPE: + { + const ConeShape* coneShape = static_cast(shape); + float radius = coneShape->GetRadius();//+coneShape->GetMargin(); + float height = coneShape->GetHeight();//+coneShape->GetMargin(); + glutSolidCone(radius,height,10,10); + useWireframeFallback = false; + break; + + } + case CONVEX_SHAPE_PROXYTYPE: + case CYLINDER_SHAPE_PROXYTYPE: + { + break; + } + default: + { + } + + }; + + } + + + if (useWireframeFallback) + { + /// for polyhedral shapes + if (shape->IsPolyhedral()) + { + PolyhedralConvexShape* polyshape = (PolyhedralConvexShape*) shape; + + + glBegin(GL_LINES); + + + int i; + for (i=0;iGetNumEdges();i++) + { + SimdPoint3 a,b; + polyshape->GetEdge(i,a,b); + + glVertex3f(a.getX(),a.getY(),a.getZ()); + glVertex3f(b.getX(),b.getY(),b.getZ()); + + + } + glEnd(); + + + + if (debugMode==IDebugDraw::DBG_DrawText) + { + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetName()); + } + + if (debugMode==IDebugDraw::DBG_DrawFeaturesText) + { + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->GetExtraDebugInfo()); + + glColor3f(1.f, 1.f, 1.f); + for (i=0;iGetNumVertices();i++) + { + SimdPoint3 vtx; + polyshape->GetVertex(i,vtx); + glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); + char buf[12]; + sprintf(buf," %d",i); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + } + + for (i=0;iGetNumPlanes();i++) + { + SimdVector3 normal; + SimdPoint3 vtx; + polyshape->GetPlane(normal,vtx,i); + SimdScalar d = vtx.dot(normal); + + glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); + char buf[12]; + sprintf(buf," plane %d",i); + BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + + } + } + } + } + + if (shape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + TriangleMeshShape* concaveMesh = (TriangleMeshShape*) shape; + //SimdVector3 aabbMax(1e30f,1e30f,1e30f); + //SimdVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f); + + extern float eye[3]; + SimdVector3 aabbMax(eye[0]+100,eye[1]+100,eye[2]+100);//1e30f,1e30f,1e30f); + SimdVector3 aabbMin(eye[0]-100,eye[1]-100,eye[2]-100);//1e30f,1e30f,1e30f); + + GlDrawcallback drawCallback; + + concaveMesh->ProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + + + } + + + + glPopMatrix(); + glPopMatrix(); + +} diff --git a/Demos/OpenGL/GL_ShapeDrawer.h b/Demos/OpenGL/GL_ShapeDrawer.h new file mode 100644 index 000000000..c37ee1906 --- /dev/null +++ b/Demos/OpenGL/GL_ShapeDrawer.h @@ -0,0 +1,31 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as 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 GL_SHAPE_DRAWER_H +#define GL_SHAPE_DRAWER_H + +class CollisionShape; +#include "SimdVector3.h" + +/// OpenGL shape drawing +class GL_ShapeDrawer +{ + public: + + static void DrawOpenGL(float* m, const CollisionShape* shape, const SimdVector3& color,int debugMode); + static void DrawCoordSystem(); + +}; + +#endif //GL_SHAPE_DRAWER_H diff --git a/Demos/OpenGL/GL_Simplex1to4.cpp b/Demos/OpenGL/GL_Simplex1to4.cpp new file mode 100644 index 000000000..0d413235b --- /dev/null +++ b/Demos/OpenGL/GL_Simplex1to4.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 "GL_Simplex1to4.h" +#include "NarrowPhaseCollision/SimplexSolverInterface.h" +#include "GL_ShapeDrawer.h" +#ifdef WIN32 +#include +#endif +#include +#include "SimdTransform.h" + +GL_Simplex1to4::GL_Simplex1to4() +:m_simplexSolver(0) +{ +} + +/// +/// Debugging method CalcClosest calculates the closest point to the origin, using m_simplexSolver +/// +void GL_Simplex1to4::CalcClosest(float* m) +{ + SimdTransform tr; + tr.setFromOpenGLMatrix(m); + + + + GL_ShapeDrawer::DrawCoordSystem(); + + if (m_simplexSolver) + { + m_simplexSolver->reset(); + bool res; + + SimdVector3 v; + SimdPoint3 pBuf[4]; + SimdPoint3 qBuf[4]; + SimdPoint3 yBuf[4]; + + + for (int i=0;iaddVertex(v,v,SimdPoint3(0.f,0.f,0.f)); + res = m_simplexSolver->closest(v); + int res = m_simplexSolver->getSimplex(pBuf, qBuf, yBuf); + + } + + + //draw v? + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glColor3f(1.f, 0.f, 0.f); + glVertex3f(0.f, 0.f, 0.f); + glVertex3f(v.x(),v.y(),v.z()); + glEnd(); + + glEnable(GL_LIGHTING); + + + } + +} diff --git a/Demos/OpenGL/GL_Simplex1to4.h b/Demos/OpenGL/GL_Simplex1to4.h new file mode 100644 index 000000000..81d628377 --- /dev/null +++ b/Demos/OpenGL/GL_Simplex1to4.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 GL_SIMPLEX_1TO4_H +#define GL_SIMPLEX_1TO4_H + +#include "CollisionShapes/Simplex1to4Shape.h" + +#include "NarrowPhaseCollision/SimplexSolverInterface.h" + +///GL_Simplex1to4 is a class to debug a Simplex Solver with 1 to 4 points. +///Can be used by GJK. +class GL_Simplex1to4 : public BU_Simplex1to4 +{ + SimplexSolverInterface* m_simplexSolver; + + public: + + GL_Simplex1to4(); + + void CalcClosest(float* m); + + void SetSimplexSolver(SimplexSolverInterface* simplexSolver) { + m_simplexSolver = simplexSolver; + } + +}; + +#endif //GL_SIMPLEX_1TO4_H diff --git a/Demos/OpenGL/GlutStuff.cpp b/Demos/OpenGL/GlutStuff.cpp new file mode 100644 index 000000000..1390efa71 --- /dev/null +++ b/Demos/OpenGL/GlutStuff.cpp @@ -0,0 +1,358 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifdef WIN32//for glut.h +#include +#endif +#include +#include +#include +#include +#include "IDebugDraw.h" +//see IDebugDraw.h for modes +static int sDebugMode = 0; + +int getDebugMode() +{ + return sDebugMode ; +} + +void setDebugMode(int mode) +{ + sDebugMode = mode; +} + + + + +#include "GlutStuff.h" + +void myinit(void) { + + GLfloat light_ambient[] = { 0.0, 0.0, 0.0, 1.0 }; + GLfloat light_diffuse[] = { 1.0, 1.0, 1.0, 1.0 }; + GLfloat light_specular[] = { 1.0, 1.0, 1.0, 1.0 }; + /* light_position is NOT default value */ + GLfloat light_position0[] = { 1.0, 1.0, 1.0, 0.0 }; + GLfloat light_position1[] = { -1.0, -1.0, -1.0, 0.0 }; + + glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT0, GL_POSITION, light_position0); + + glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient); + glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse); + glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular); + glLightfv(GL_LIGHT1, GL_POSITION, light_position1); + + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); + + + glShadeModel(GL_SMOOTH); + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + + glClearColor(0.8,0.8,0.8,0); + + // glEnable(GL_CULL_FACE); + // glCullFace(GL_BACK); +} + + +static float DISTANCE = 15; +void setCameraDistance(float dist) +{ + DISTANCE = dist; +} + +static float ele = 0, azi = 0; +float eye[3] = {0, 0, DISTANCE}; +static float center[3] = {0, 0, 0}; +static const double SCALE_BOTTOM = 0.5; +static const double SCALE_FACTOR = 2; + +bool stepping= true; +bool singleStep = false; + + +static bool idle = false; + +void toggleIdle() { + + + if (idle) { + glutIdleFunc(clientMoveAndDisplay); + idle = false; + } + else { + glutIdleFunc(0); + idle = true; + } +} + + +void setCamera() { + + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + float rele = ele * 0.01745329251994329547;// rads per deg + float razi = azi * 0.01745329251994329547;// rads per deg + eye[0] = DISTANCE * sin(razi) * cos(rele); + eye[1] = DISTANCE * sin(rele); + eye[2] = DISTANCE * cos(razi) * cos(rele); + + + glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 100.0); + gluLookAt(eye[0], eye[1], eye[2], + center[0], center[1], center[2], + 0, 1, 0); + glMatrixMode(GL_MODELVIEW); +} + + + +const float STEPSIZE = 5; + +void stepLeft() { azi -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void stepRight() { azi += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepFront() { ele += STEPSIZE; if (azi >= 360) azi -= 360; setCamera(); } +void stepBack() { ele -= STEPSIZE; if (azi < 0) azi += 360; setCamera(); } +void zoomIn() { DISTANCE -= 1; setCamera(); } +void zoomOut() { DISTANCE += 1; setCamera(); } + + +int glutScreenWidth = 0; +int glutScreenHeight = 0; + +void myReshape(int w, int h) { + glutScreenWidth = w; + glutScreenHeight = h; + + glViewport(0, 0, w, h); + setCamera(); +} + +int lastKey = 0; + +void defaultKeyboard(unsigned char key, int x, int y) +{ + lastKey = 0; + + switch (key) + { + case 'q' : exit(0); break; + + case 'l' : stepLeft(); break; + case 'r' : stepRight(); break; + case 'f' : stepFront(); break; + case 'b' : stepBack(); break; + case 'z' : zoomIn(); break; + case 'x' : zoomOut(); break; + case 'i' : toggleIdle(); break; + case 'h': + if (sDebugMode & IDebugDraw::DBG_NoHelpText) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_NoHelpText); + else + sDebugMode |= IDebugDraw::DBG_NoHelpText; + break; + + case 'w': + if (sDebugMode & IDebugDraw::DBG_DrawWireframe) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DrawWireframe); + else + sDebugMode |= IDebugDraw::DBG_DrawWireframe; + break; + + case 'p': + if (sDebugMode & IDebugDraw::DBG_ProfileTimings) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_ProfileTimings); + else + sDebugMode |= IDebugDraw::DBG_ProfileTimings; + break; + + case 'm': + if (sDebugMode & IDebugDraw::DBG_EnableSatComparison) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_EnableSatComparison); + else + sDebugMode |= IDebugDraw::DBG_EnableSatComparison; + break; + + case 'n': + if (sDebugMode & IDebugDraw::DBG_DisableBulletLCP) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DisableBulletLCP); + else + sDebugMode |= IDebugDraw::DBG_DisableBulletLCP; + break; + + case 't' : + if (sDebugMode & IDebugDraw::DBG_DrawText) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DrawText); + else + sDebugMode |= IDebugDraw::DBG_DrawText; + break; + case 'y': + if (sDebugMode & IDebugDraw::DBG_DrawFeaturesText) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DrawFeaturesText); + else + sDebugMode |= IDebugDraw::DBG_DrawFeaturesText; + break; + case 'a': + if (sDebugMode & IDebugDraw::DBG_DrawAabb) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DrawAabb); + else + sDebugMode |= IDebugDraw::DBG_DrawAabb; + break; + case 'c' : + if (sDebugMode & IDebugDraw::DBG_DrawContactPoints) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_DrawContactPoints); + else + sDebugMode |= IDebugDraw::DBG_DrawContactPoints; + break; + + case 'd' : + if (sDebugMode & IDebugDraw::DBG_NoDeactivation) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_NoDeactivation); + else + sDebugMode |= IDebugDraw::DBG_NoDeactivation; + break; + + + + case 'o' : + { + stepping = !stepping; + break; + } + case 's' : clientMoveAndDisplay(); break; +// case ' ' : newRandom(); break; + case ' ': + clientResetScene(); + break; + case '1': + { + if (sDebugMode & IDebugDraw::DBG_EnableCCD) + sDebugMode = sDebugMode & (~IDebugDraw::DBG_EnableCCD); + else + sDebugMode |= IDebugDraw::DBG_EnableCCD; + break; + } + default: +// std::cout << "unused key : " << key << std::endl; + break; + } + + glutPostRedisplay(); + +} + + +void mySpecial(int key, int x, int y) +{ + switch (key) + { + case GLUT_KEY_LEFT : stepLeft(); break; + case GLUT_KEY_RIGHT : stepRight(); break; + case GLUT_KEY_UP : stepFront(); break; + case GLUT_KEY_DOWN : stepBack(); break; + case GLUT_KEY_PAGE_UP : zoomIn(); break; + case GLUT_KEY_PAGE_DOWN : zoomOut(); break; + case GLUT_KEY_HOME : toggleIdle(); break; + default: +// std::cout << "unused (special) key : " << key << std::endl; + break; + } + + glutPostRedisplay(); + +} + + +void goodbye( void) +{ + printf("goodbye \n"); + exit(0); +} + + +void menu(int choice) +{ + static int fullScreen = 0; + static int px, py, sx, sy; + + switch(choice) { + case 1: + if (fullScreen == 1) { + glutPositionWindow(px,py); + glutReshapeWindow(sx,sy); + glutChangeToMenuEntry(1,"Full Screen",1); + fullScreen = 0; + } else { + px=glutGet((GLenum)GLUT_WINDOW_X); + py=glutGet((GLenum)GLUT_WINDOW_Y); + sx=glutGet((GLenum)GLUT_WINDOW_WIDTH); + sy=glutGet((GLenum)GLUT_WINDOW_HEIGHT); + glutFullScreen(); + glutChangeToMenuEntry(1,"Close Full Screen",1); + fullScreen = 1; + } + break; + case 2: + toggleIdle(); + break; + case 3: + goodbye(); + break; + default: + break; + } +} + +void createMenu() +{ + glutCreateMenu(menu); + glutAddMenuEntry("Full Screen", 1); + glutAddMenuEntry("Toggle Idle (Start/Stop)", 2); + glutAddMenuEntry("Quit", 3); + glutAttachMenu(GLUT_RIGHT_BUTTON); +} + + + +int glutmain(int argc, char **argv,int width,int height,const char* title) { + + + glutInit(&argc, argv); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH); + glutInitWindowPosition(0, 0); + glutInitWindowSize(width, height); + glutCreateWindow(title); + + myinit(); + glutKeyboardFunc(clientKeyboard); + glutSpecialFunc(mySpecial); + glutReshapeFunc(myReshape); + //createMenu(); + glutIdleFunc(clientMoveAndDisplay); + glutMouseFunc(clientMouseFunc); + glutMotionFunc(clientMotionFunc); + glutDisplayFunc( clientDisplay ); + + + glutMainLoop(); + return 0; +} diff --git a/Demos/OpenGL/GlutStuff.h b/Demos/OpenGL/GlutStuff.h new file mode 100644 index 000000000..7a139a82a --- /dev/null +++ b/Demos/OpenGL/GlutStuff.h @@ -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. +*/ +#ifndef GLUT_STUFF_H +#define GLUT_STUFF_H + +//to be implemented by the demo +void clientDisplay(); +void clientMoveAndDisplay(); +void clientResetScene(); + + +int glutmain(int argc, char **argv,int width,int height,const char* title); + +void setCameraDistance(float dist); +int getDebugMode(); +void setDebugMode(int mode); + +void defaultKeyboard(unsigned char key, int x, int y); +void clientKeyboard(unsigned char key, int x, int y); + +void clientMouseFunc(int button, int state, int x, int y); +void clientMotionFunc(int x,int y); +#endif //GLUT_STUFF_H diff --git a/Demos/OpenGL/Jamfile b/Demos/OpenGL/Jamfile new file mode 100644 index 000000000..36ab413ce --- /dev/null +++ b/Demos/OpenGL/Jamfile @@ -0,0 +1,8 @@ +SubDir TOP Demos OpenGL ; + +if $(GLUT.AVAILABLE) = "yes" +{ + Description bulletopenglsupport : "Bullet OpenGL support" ; + Library bulletopenglsupport : [ Wildcard *.h *.cpp ] : noinstall ; + ExternalLibs bulletopenglsupport : GLUT ; +} diff --git a/Demos/OpenGL/RenderTexture.cpp b/Demos/OpenGL/RenderTexture.cpp new file mode 100644 index 000000000..9af21d29f --- /dev/null +++ b/Demos/OpenGL/RenderTexture.cpp @@ -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. +*/ + +#include "RenderTexture.h" +#include +#include "BMF_FontData.h" + +RenderTexture::RenderTexture(int width,int height) +:m_height(height),m_width(width) +{ + m_buffer = new unsigned char[m_width*m_height*4]; + + //clear screen + memset(m_buffer,0,m_width*m_height*4); + + //clear screen version 2 + for (int x=0;xchars[c]; + + if (cd.data_offset!=-1) { + unsigned char* bitmap = &fontData->bitmap_data[cd.data_offset]; + for (int y=0;y>=1; + } + } + } + rasterposx+= cd.advance; + } +} + +RenderTexture::~RenderTexture() +{ + delete [] m_buffer; +} + + + diff --git a/Demos/OpenGL/RenderTexture.h b/Demos/OpenGL/RenderTexture.h new file mode 100644 index 000000000..3c30e624c --- /dev/null +++ b/Demos/OpenGL/RenderTexture.h @@ -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. +*/ + +#ifndef RENDER_TEXTURE_H +#define RENDER_TEXTURE_H + +#include "SimdVector3.h" +#include "BMF_FontData.h" + +/// +///RenderTexture provides a software-render context (setpixel/printf) +/// +class RenderTexture +{ + int m_height; + int m_width; + unsigned char* m_buffer; + +public: + + RenderTexture(int width,int height); + ~RenderTexture(); + + inline void SetPixel(int x,int y,const SimdVector4& rgba) + { + unsigned char* pixel = &m_buffer[ (x+y*m_width) * 4]; + + pixel[0] = (unsigned char)(255*rgba.getX()); + pixel[1] = (unsigned char)(255*rgba.getY()); + pixel[2] = (unsigned char)(255*rgba.getZ()); + pixel[3] = (unsigned char)(255*rgba.getW()); + } + + const unsigned char* GetBuffer() const { return m_buffer;} + int GetWidth() const { return m_width;} + int GetHeight() const { return m_height;} + void Printf(char* str, BMF_FontData* fontData, int startx = 0,int starty=0); + +}; + +#endif //RENDER_TEXTURE_H diff --git a/Demos/Raytracer/Jamfile b/Demos/Raytracer/Jamfile new file mode 100644 index 000000000..b680d5578 --- /dev/null +++ b/Demos/Raytracer/Jamfile @@ -0,0 +1,3 @@ +SubDir TOP Demos Raytracer ; + +BulletDemo Raytracer : [ Wildcard *.h *.cpp ] ; diff --git a/Demos/Raytracer/Raytracer.cpp b/Demos/Raytracer/Raytracer.cpp new file mode 100644 index 000000000..d63e76f93 --- /dev/null +++ b/Demos/Raytracer/Raytracer.cpp @@ -0,0 +1,412 @@ +/* +* Copyright (c) 2005 Erwin Coumans +* +* 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. +*/ + + + +/* +Raytracer uses the Convex Raycast to visualize the Collision Shapes/Minkowski Sum. +Very basic raytracer, rendering into a texture. +*/ + +#include "GL_Simplex1to4.h" +#include "SimdQuaternion.h" +#include "SimdTransform.h" +#include "GL_ShapeDrawer.h" +#ifdef WIN32 //needed for glut.h +#include +#endif +#include +#include "GlutStuff.h" + +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" +#include "NarrowPhaseCollision/SubSimplexConvexCast.h" +#include "NarrowPhaseCollision/GjkConvexCast.h" +#include "NarrowPhaseCollision/ContinuousConvexCollision.h" +#include "NarrowPhaseCollision/BU_CollisionPair.h" + + + +#include "CollisionShapes/SphereShape.h" +#include "CollisionShapes/MultiSphereShape.h" +#include "CollisionShapes/ConvexHullShape.h" +#include "CollisionShapes/BoxShape.h" +#include "CollisionShapes/Simplex1to4Shape.h" +#include "CollisionShapes/ConeShape.h" +#include "CollisionShapes/CylinderShape.h" +#include "CollisionShapes/MinkowskiSumShape.h" + + + +#include "RenderTexture.h" + +VoronoiSimplexSolver simplexSolver; + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 4; + +/// simplex contains the vertices, and some extra code to draw and debug +GL_Simplex1to4 simplex; + +ConvexShape* shapePtr[maxNumObjects]; +SimdTransform transforms[maxNumObjects]; + +RenderTexture* raytracePicture = 0; + +int screenWidth = 128; +int screenHeight = 128; +GLuint glTextureId; + +SphereShape mySphere(1); +BoxShape myBox(SimdVector3(0.4f,0.4f,0.4f)); +CylinderShape myCylinder(SimdVector3(0.3f,0.3f,0.3f)); +ConeShape myCone(1,1); + +MinkowskiSumShape myMink(&myCylinder,&myBox); + + +/// +/// +/// +int main(int argc,char** argv) +{ + raytracePicture = new RenderTexture(screenWidth,screenHeight); + + myBox.SetMargin(0.02f); + myCone.SetMargin(0.2f); + + simplex.SetSimplexSolver(&simplexSolver); + simplex.AddVertex(SimdPoint3(-1,0,-1)); + simplex.AddVertex(SimdPoint3(1,0,-1)); + simplex.AddVertex(SimdPoint3(0,0,1)); + simplex.AddVertex(SimdPoint3(0,1,0)); + + + /// convex hull of 5 spheres +#define NUM_SPHERES 5 + SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); + SimdVector3 positions[NUM_SPHERES] = { + SimdVector3(-1.2f, -0.3f, 0.f), + SimdVector3(0.8f, -0.3f, 0.f), + SimdVector3(0.5f, 0.6f, 0.f), + SimdVector3(-0.5f, 0.6f, 0.f), + SimdVector3(0.f, 0.f, 0.f) + }; + SimdScalar radi[NUM_SPHERES] = { 0.35f,0.35f,0.45f,0.40f,0.40f }; + + MultiSphereShape multiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); + + ConvexHullShape convexHullShape(positions,3); + + + //choose shape + shapePtr[0] = &myCone; + shapePtr[1] =&simplex; + shapePtr[2] =&convexHullShape; + shapePtr[3] =&myMink;//myBox; + + simplex.SetMargin(0.3f); + + setCameraDistance(6.f); + + return glutmain(argc, argv,screenWidth,screenHeight,"Minkowski-Sum Raytracer Demo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + +int once = 1; +extern float eye[3]; + +void clientDisplay(void) +{ + + for (int i=0;iSetPixel(x,y,rgba); + } + } + + + ConvexCast::CastResult rayResult; + SimdTransform rayToTrans; + rayToTrans.setIdentity(); + SimdVector3 rayTo; + for (int x=0;x 1.f) + light = 1.f; + + rgba = SimdVector4(light,light,light,1.f); + raytracePicture->SetPixel(x,y,rgba); + } else + { + //clear is already done + //rgba = SimdVector4(0.f,0.f,0.f,0.f); + //raytracePicture->SetPixel(x,y,rgba); + + } + + + } + } + } + +#define TEST_PRINTF +#ifdef TEST_PRINTF + + + extern BMF_FontData BMF_font_helv10; + + raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10); + char buffer[256]; + sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects); + raytracePicture->Printf(buffer,&BMF_font_helv10,0,10); + + +#endif //TEST_PRINTF + + glMatrixMode(GL_PROJECTION); + glPushMatrix(); + glLoadIdentity(); + glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0); + + glMatrixMode(GL_MODELVIEW); + glPushMatrix(); + glLoadIdentity(); // Reset The Modelview Matrix + glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units + + + + glEnable(GL_TEXTURE_2D); + glBindTexture(GL_TEXTURE_2D,glTextureId ); + + const unsigned char *ptr = raytracePicture->GetBuffer(); + glTexImage2D(GL_TEXTURE_2D, + 0, + GL_RGBA, + raytracePicture->GetWidth(),raytracePicture->GetHeight(), + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + ptr); + + + glEnable (GL_BLEND); + glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glColor4f (1,1,1,1); // alpha=0.5=half visible + + glBegin(GL_QUADS); + glTexCoord2f(0.0f, 0.0f); + glVertex2f(-1,1); + glTexCoord2f(1.0f, 0.0f); + glVertex2f(1,1); + glTexCoord2f(1.0f, 1.0f); + glVertex2f(1,-1); + glTexCoord2f(0.0f, 1.0f); + glVertex2f(-1,-1); + glEnd(); + + + + glMatrixMode(GL_MODELVIEW); + glPopMatrix(); + glMatrixMode(GL_PROJECTION); + glPopMatrix(); + glMatrixMode(GL_MODELVIEW); + +#endif //RAYRACER + + glDisable(GL_TEXTURE_2D); + glDisable(GL_DEPTH_TEST); + + GL_ShapeDrawer::DrawCoordSystem(); + + glPushMatrix(); + + + + + /* + /// normal opengl rendering + float m[16]; + int i; + + for (i=0;i +#endif +#include +#include "GlutStuff.h" + + + +#include "NarrowPhaseCollision/VoronoiSimplexSolver.h" + +VoronoiSimplexSolver simplexSolver; + + + +float yaw=0.f,pitch=0.f,roll=0.f; +const int maxNumObjects = 4; +const int numObjects = 1; +int screenWidth = 640.f; +int screenHeight = 480.f; +/// simplex contains the vertices, and some extra code to draw and debug +GL_Simplex1to4 simplex; + + +PolyhedralConvexShape* shapePtr[maxNumObjects]; + + +/// +/// +/// +int main(int argc,char** argv) +{ + + simplex.SetSimplexSolver(&simplexSolver); + + simplex.AddVertex(SimdPoint3(-2,0,-2)); + simplex.AddVertex(SimdPoint3(2,0,-2)); + simplex.AddVertex(SimdPoint3(0,0,2)); + simplex.AddVertex(SimdPoint3(0,2,0)); + + shapePtr[0] = &simplex; + + SimdTransform tr; + tr.setIdentity(); + + return glutmain(argc, argv,screenWidth,screenHeight,"SimplexDemo"); +} + +//to be implemented by the demo + +void clientMoveAndDisplay() +{ + + clientDisplay(); +} + + + +void clientDisplay(void) { + + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glDisable(GL_LIGHTING); + + GL_ShapeDrawer::DrawCoordSystem(); + + float m[16]; + int i; + + for (i=0;i