archive/blender-archive
Archived
1
1
Fork 0
Code Pull Requests Packages Activity

upgraded Bullet rigidbody physics to latest version 1.9

Browse Source
This commit is contained in:
Erwin Coumans
2006-08-28 06:44:29 +00:00
parent 81d7cd967d
commit 6ce5d97c56
95 changed files with 4613 additions and 2501 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.cpp vendored
View File

@@ -21,11 +21,12 @@
#include <assert.h>
BroadphaseProxy* AxisSweep3::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr )
BroadphaseProxy* AxisSweep3::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
{
unsigned short handleId = AddHandle(min,max, userPtr);
unsigned short handleId = AddHandle(min,max, userPtr,collisionFilterGroup,collisionFilterMask);
Handle* handle = GetHandle(handleId);
return handle;
}
@@ -47,6 +48,7 @@ void AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const
AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps)
:OverlappingPairCache(maxOverlaps)
{
//assert(bounds.HasVolume());
@@ -156,7 +158,7 @@ void AxisSweep3::FreeHandle(unsigned short handle)
unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner)
unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
{
// quantize the bounds
unsigned short min[3], max[3];
@@ -169,10 +171,11 @@ unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3&
Handle* pHandle = GetHandle(handle);
pHandle->m_handleId = handle;
//pHandle->m_pOverlaps = 0;
pHandle->m_clientObject = pOwner;
pHandle->m_collisionFilterGroup = collisionFilterGroup;
pHandle->m_collisionFilterMask = collisionFilterMask;
// compute current limit of edge arrays
int limit = m_numHandles * 2;

11
extern/bullet/Bullet/BroadphaseCollision/AxisSweep3.h vendored
View File

@@ -21,10 +21,13 @@
#include "SimdPoint3.h"
#include "SimdVector3.h"
#include "SimpleBroadphase.h"
#include "OverlappingPairCache.h"
#include "BroadphaseProxy.h"
class AxisSweep3 : public SimpleBroadphase
/// 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 OverlappingPairCache
{
public:
@@ -96,14 +99,14 @@ public:
//this is replace by sweep and prune
}
unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner);
unsigned short AddHandle(const SimdPoint3& aabbMin,const SimdPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask);
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 BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
virtual void DestroyProxy(BroadphaseProxy* proxy);
virtual void SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax);

4
extern/bullet/Bullet/BroadphaseCollision/BroadphaseInterface.h vendored
View File

@@ -29,11 +29,11 @@ class BroadphaseInterface
public:
virtual ~BroadphaseInterface() {}
virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ) =0;
virtual BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =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;
};

17
extern/bullet/Bullet/BroadphaseCollision/BroadphaseProxy.h vendored
View File

@@ -27,6 +27,7 @@ enum BroadphaseNativeTypes
BOX_SHAPE_PROXYTYPE,
TRIANGLE_SHAPE_PROXYTYPE,
TETRAHEDRAL_SHAPE_PROXYTYPE,
CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
CONVEX_HULL_SHAPE_PROXYTYPE,
//implicit convex shapes
IMPLICIT_CONVEX_SHAPES_START_HERE,
@@ -42,6 +43,10 @@ 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,
STATIC_PLANE_PROXYTYPE,
CONCAVE_SHAPES_END_HERE,
COMPOUND_SHAPE_PROXYTYPE,
MAX_BROADPHASE_COLLISION_TYPES
};
@@ -53,12 +58,16 @@ struct BroadphaseProxy
//Usually the client CollisionObject or Rigidbody class
void* m_clientObject;
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
//used for memory pools
BroadphaseProxy() :m_clientObject(0){}
BroadphaseProxy(int shapeType,void* userPtr)
:m_clientObject(userPtr)
//m_clientObjectType(shapeType)
BroadphaseProxy(void* userPtr,short int collisionFilterGroup, short int collisionFilterMask)
:m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask)
{
}

8
extern/bullet/Bullet/BroadphaseCollision/Dispatcher.h vendored
View File

@@ -21,6 +21,7 @@ struct BroadphaseProxy;
class RigidBody;
struct CollisionObject;
class ManifoldResult;
class OverlappingPairCache;
enum CollisionDispatcherId
{
@@ -80,10 +81,17 @@ public:
virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) = 0;
virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1)=0;
virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold) =0;
virtual void ReleaseManifoldResult(ManifoldResult*)=0;
virtual void DispatchAllCollisionPairs(struct BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)=0;
virtual int GetNumManifolds() const = 0;
virtual PersistentManifold* GetManifoldByIndexInternal(int index) = 0;
};

151
extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.cpp vendored Normal file
View File

@@ -0,0 +1,151 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "OverlappingPairCache.h"
#include "Dispatcher.h"
#include "CollisionAlgorithm.h"
OverlappingPairCache::OverlappingPairCache(int maxOverlap):
m_blockedForChanges(false),
m_NumOverlapBroadphasePair(0),
m_maxOverlap(maxOverlap)
{
m_OverlappingPairs = new BroadphasePair[maxOverlap];
}
OverlappingPairCache::~OverlappingPairCache()
{
delete [] m_OverlappingPairs;
}
void OverlappingPairCache::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--;
}
void OverlappingPairCache::CleanOverlappingPair(BroadphasePair& pair)
{
for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
{
if (pair.m_algorithms[dispatcherId])
{
{
delete pair.m_algorithms[dispatcherId];
pair.m_algorithms[dispatcherId]=0;
}
}
}
}
void OverlappingPairCache::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
{
//don't add overlap with own
assert(proxy0 != proxy1);
if (!NeedsCollision(proxy0,proxy1))
return;
BroadphasePair pair(*proxy0,*proxy1);
m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
int i;
for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
{
assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
}
if (m_NumOverlapBroadphasePair >= m_maxOverlap)
{
//printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
#ifdef DEBUG
assert(0);
#endif
} else
{
m_NumOverlapBroadphasePair++;
}
}
BroadphasePair* OverlappingPairCache::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 OverlappingPairCache::CleanProxyFromPairs(BroadphaseProxy* proxy)
{
for (int i=0;i<m_NumOverlapBroadphasePair;i++)
{
BroadphasePair& pair = m_OverlappingPairs[i];
if (pair.m_pProxy0 == proxy ||
pair.m_pProxy1 == proxy)
{
CleanOverlappingPair(pair);
}
}
}
void OverlappingPairCache::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);
}
}
}

85
extern/bullet/Bullet/BroadphaseCollision/OverlappingPairCache.h vendored Normal file
View File

@@ -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 OVERLAPPING_PAIR_CACHE_H
#define OVERLAPPING_PAIR_CACHE_H
#include "BroadphaseInterface.h"
#include "BroadphaseProxy.h"
#include "SimdPoint3.h"
///OverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or SimpleBroadphase
class OverlappingPairCache : public BroadphaseInterface
{
BroadphasePair* m_OverlappingPairs;
int m_NumOverlapBroadphasePair;
int m_maxOverlap;
//during the dispatch, check that user doesn't destroy/create proxy
bool m_blockedForChanges;
public:
OverlappingPairCache(int maxOverlap);
virtual ~OverlappingPairCache();
int GetNumOverlappingPairs() const
{
return m_NumOverlapBroadphasePair;
}
BroadphasePair& GetOverlappingPair(int index)
{
return m_OverlappingPairs[index];
}
void RemoveOverlappingPair(BroadphasePair& pair);
void CleanOverlappingPair(BroadphasePair& pair);
void AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
BroadphasePair* FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
void CleanProxyFromPairs(BroadphaseProxy* proxy);
void RemoveOverlappingPairsContainingProxy(BroadphaseProxy* proxy);
inline bool NeedsCollision(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) const
{
bool collides = proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask;
collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
return collides;
}
virtual void RefreshOverlappingPairs() =0;
};
#endif //OVERLAPPING_PAIR_CACHE_H

180
extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.cpp vendored
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
*/
#include "SimpleBroadphase.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "BroadphaseCollision/CollisionAlgorithm.h"
#include <BroadphaseCollision/Dispatcher.h>
#include <BroadphaseCollision/CollisionAlgorithm.h>
#include "SimdVector3.h"
#include "SimdTransform.h"
@@ -36,19 +36,16 @@ void SimpleBroadphase::validate()
}
SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
:m_firstFreeProxy(0),
:OverlappingPairCache(maxOverlap),
m_firstFreeProxy(0),
m_numProxies(0),
m_blockedForChanges(false),
m_NumOverlapBroadphasePair(0),
m_maxProxies(maxProxies),
m_maxOverlap(maxOverlap)
m_maxProxies(maxProxies)
{
m_proxies = new SimpleBroadphaseProxy[maxProxies];
m_freeProxies = new int[maxProxies];
m_pProxies = new SimpleBroadphaseProxy*[maxProxies];
m_OverlappingPairs = new BroadphasePair[maxOverlap];
int i;
for (i=0;i<m_maxProxies;i++)
@@ -62,7 +59,6 @@ SimpleBroadphase::~SimpleBroadphase()
delete[] m_proxies;
delete []m_freeProxies;
delete [] m_pProxies;
delete [] m_OverlappingPairs;
/*int i;
for (i=m_numProxies-1;i>=0;i--)
@@ -74,7 +70,7 @@ SimpleBroadphase::~SimpleBroadphase()
}
BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr)
BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask)
{
if (m_numProxies >= m_maxProxies)
{
@@ -84,7 +80,7 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const
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);
SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr,collisionFilterGroup,collisionFilterMask);
m_firstFreeProxy++;
SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
@@ -99,19 +95,7 @@ BroadphaseProxy* SimpleBroadphase::CreateProxy( const SimdVector3& min, const
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)
{
@@ -148,89 +132,13 @@ void SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin
sbp->m_max = aabbMax;
}
void SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair)
{
for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
{
if (pair.m_algorithms[dispatcherId])
{
{
delete pair.m_algorithms[dispatcherId];
pair.m_algorithms[dispatcherId]=0;
}
}
}
}
void SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
{
for (int i=0;i<m_NumOverlapBroadphasePair;i++)
{
BroadphasePair& pair = m_OverlappingPairs[i];
if (pair.m_pProxy0 == proxy ||
pair.m_pProxy1 == proxy)
{
CleanOverlappingPair(pair);
}
}
}
void SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
{
//don't add overlap with own
assert(proxy0 != proxy1);
BroadphasePair pair(*proxy0,*proxy1);
m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
int i;
for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
{
assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
}
if (m_NumOverlapBroadphasePair >= 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)
{
@@ -265,9 +173,10 @@ void SimpleBroadphase::RefreshOverlappingPairs()
}
//then remove non-overlapping ones
for (i=0;i<m_NumOverlapBroadphasePair;i++)
for (i=0;i<GetNumOverlappingPairs();i++)
{
BroadphasePair& pair = m_OverlappingPairs[i];
BroadphasePair& pair = GetOverlappingPair(i);
SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
if (!AabbOverlap(proxy0,proxy1))
@@ -280,69 +189,4 @@ void SimpleBroadphase::RefreshOverlappingPairs()
}
void SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)
{
m_blockedForChanges = true;
int i;
int dispatcherId = dispatcher.GetUniqueId();
RefreshOverlappingPairs();
for (i=0;i<m_NumOverlapBroadphasePair;i++)
{
BroadphasePair& pair = m_OverlappingPairs[i];
if (dispatcherId>= 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;
}

37
extern/bullet/Bullet/BroadphaseCollision/SimpleBroadphase.h vendored
View File

@@ -16,12 +16,9 @@ subject to the following restrictions:
#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"
#include "OverlappingPairCache.h"
struct SimpleBroadphaseProxy : public BroadphaseProxy
{
@@ -30,8 +27,8 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy
SimpleBroadphaseProxy() {};
SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr)
:BroadphaseProxy(shapeType,userPtr),
SimpleBroadphaseProxy(const SimdPoint3& minpt,const SimdPoint3& maxpt,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
:BroadphaseProxy(userPtr,collisionFilterGroup,collisionFilterMask),
m_min(minpt),m_max(maxpt)
{
}
@@ -40,7 +37,7 @@ struct SimpleBroadphaseProxy : public BroadphaseProxy
};
///SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks
class SimpleBroadphase : public BroadphaseInterface
class SimpleBroadphase : public OverlappingPairCache
{
SimpleBroadphaseProxy* m_proxies;
@@ -50,14 +47,10 @@ class SimpleBroadphase : public BroadphaseInterface
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)
{
@@ -70,24 +63,24 @@ class SimpleBroadphase : public BroadphaseInterface
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 BroadphaseProxy* CreateProxy( const SimdVector3& min, const SimdVector3& max,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask);
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);
};

42
extern/bullet/Bullet/CollisionDispatch/CollisionCreateFunc.h vendored Normal file
View File

@@ -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 COLLISION_CREATE_FUNC
#define COLLISION_CREATE_FUNC
#include <vector>
typedef std::vector<struct CollisionObject*> CollisionObjectArray;
class CollisionAlgorithm;
struct BroadphaseProxy;
struct CollisionAlgorithmCreateFunc
{
bool m_swapped;
CollisionAlgorithmCreateFunc()
:m_swapped(false)
{
}
virtual ~CollisionAlgorithmCreateFunc(){};
virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
{
return 0;
}
};
#endif //COLLISION_CREATE_FUNC

200
extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.cpp vendored
View File

@@ -21,40 +21,15 @@ subject to the following restrictions:
#include "CollisionDispatch/ConvexConvexAlgorithm.h"
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
#include "CollisionDispatch/CompoundCollisionAlgorithm.h"
#include "CollisionShapes/CollisionShape.h"
#include "CollisionDispatch/CollisionObject.h"
#include <algorithm>
#include "BroadphaseCollision/OverlappingPairCache.h"
int gNumManifold = 0;
void CollisionDispatcher::FindUnions()
{
if (m_useIslands)
{
for (int i=0;i<GetNumManifolds();i++)
{
const PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//static objects (invmass 0.f) don't merge !
const CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
const CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
if (colObj0 && colObj1 && NeedsResponse(*colObj0,*colObj1))
{
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
{
m_unionFind.unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
}
@@ -79,7 +54,9 @@ CollisionDispatcher::CollisionDispatcher ():
PersistentManifold* CollisionDispatcher::GetNewManifold(void* b0,void* b1)
{
gNumManifold++;
//printf("GetNewManifoldResult: gNumManifold %d\n",gNumManifold);
//ASSERT(gNumManifold < 65535);
CollisionObject* body0 = (CollisionObject*)b0;
CollisionObject* body1 = (CollisionObject*)b1;
@@ -119,96 +96,6 @@ void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
}
//
// todo: this is random access, it can be walked 'cache friendly'!
//
void CollisionDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
{
int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++)
{
std::vector<PersistentManifold*> islandmanifold;
//int numSleeping = 0;
bool allSleeping = true;
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if (colObj0->GetActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
for (i=0;i<GetNumManifolds();i++)
{
PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
{
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
{
if (NeedsResponse(*colObj0,*colObj1))
islandmanifold.push_back(manifold);
}
}
}
if (allSleeping)
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
colObj0->SetActivationState( ISLAND_SLEEPING );
}
}
} else
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
{
colObj0->SetActivationState( WANTS_DEACTIVATION);
}
}
}
/// Process the actual simulation, only if not sleeping/deactivated
if (islandmanifold.size())
{
callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
}
}
}
}
@@ -236,6 +123,17 @@ CollisionAlgorithm* CollisionDispatcher::InternalFindAlgorithm(BroadphaseProxy&
return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
}
if (body0->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
} else
{
if (body1->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
}
}
//failed to find an algorithm
return new EmptyAlgorithm(ci);
@@ -291,3 +189,69 @@ void CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*)
{
}
void CollisionDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)
{
//m_blockedForChanges = true;
int i;
int dispatcherId = GetUniqueId();
for (i=0;i<numPairs;i++)
{
BroadphasePair& pair = pairs[i];
if (dispatcherId>= 0)
{
//dispatcher will keep algorithms persistent in the collision pair
if (!pair.m_algorithms[dispatcherId])
{
pair.m_algorithms[dispatcherId] = 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 = 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;
}

46
extern/bullet/Bullet/CollisionDispatch/CollisionDispatcher.h vendored
View File

@@ -18,32 +18,19 @@ subject to the following restrictions:
#include "BroadphaseCollision/Dispatcher.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionDispatch/UnionFind.h"
#include "CollisionDispatch/ManifoldResult.h"
#include "BroadphaseCollision/BroadphaseProxy.h"
#include <vector>
class IDebugDraw;
typedef std::vector<struct CollisionObject*> CollisionObjectArray;
class OverlappingPairCache;
struct CollisionAlgorithmCreateFunc
{
bool m_swapped;
CollisionAlgorithmCreateFunc()
:m_swapped(false)
{
}
virtual ~CollisionAlgorithmCreateFunc(){};
#include "CollisionCreateFunc.h"
virtual CollisionAlgorithm* CreateCollisionAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
{
return 0;
}
};
///CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
@@ -53,7 +40,7 @@ class CollisionDispatcher : public Dispatcher
std::vector<PersistentManifold*> m_manifoldsPtr;
UnionFind m_unionFind;
bool m_useIslands;
@@ -63,14 +50,9 @@ class CollisionDispatcher : public Dispatcher
public:
UnionFind& GetUnionFind() { return m_unionFind;}
struct IslandCallback
{
virtual ~IslandCallback() {};
virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0;
};
int GetNumManifolds() const
@@ -88,14 +70,6 @@ public:
return m_manifoldsPtr[index];
}
void InitUnionFind(int n)
{
if (m_useIslands)
m_unionFind.reset(n);
}
void FindUnions();
int m_count;
CollisionDispatcher ();
@@ -106,8 +80,6 @@ public:
virtual void ReleaseManifold(PersistentManifold* manifold);
virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback);
///allows the user to get contact point callbacks
virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold);
@@ -131,6 +103,8 @@ public:
virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo);
};

5
extern/bullet/Bullet/CollisionDispatch/CollisionObject.cpp vendored
View File

@@ -21,8 +21,11 @@ CollisionObject::CollisionObject()
m_deactivationTime(0.f),
m_broadphaseHandle(0),
m_collisionShape(0),
m_hitFraction(1.f)
m_hitFraction(1.f),
m_ccdSweptShereRadius(0.f),
m_ccdSquareMotionTreshold(0.f)
{
m_cachedInvertedWorldTransform.setIdentity();
}

37
extern/bullet/Bullet/CollisionDispatch/CollisionObject.h vendored
View File

@@ -28,6 +28,9 @@ subject to the following restrictions:
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;
@@ -35,12 +38,14 @@ struct CollisionObject
//m_interpolationWorldTransform is used for CCD and interpolation
//it can be either previous or future (predicted) transform
SimdTransform m_interpolationWorldTransform;
SimdTransform m_cachedInvertedWorldTransform;
enum CollisionFlags
{
isStatic = 1,
noContactResponse = 2,
customMaterialCallback = 4,//this allows per-triangle material (friction/restitution)
};
int m_collisionFlags;
@@ -49,13 +54,22 @@ struct CollisionObject
int m_activationState1;
float m_deactivationTime;
SimdScalar m_friction;
SimdScalar m_restitution;
BroadphaseProxy* m_broadphaseHandle;
CollisionShape* m_collisionShape;
void* m_userPointer;//not use by Bullet internally
//time of impact calculation
///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;
@@ -91,6 +105,23 @@ struct CollisionObject
return ((GetActivationState() != ISLAND_SLEEPING) && (GetActivationState() != DISABLE_SIMULATION));
}
void setRestitution(float rest)
{
m_restitution = rest;
}
float getRestitution() const
{
return m_restitution;
}
void setFriction(float frict)
{
m_friction = frict;
}
float getFriction() const
{
return m_friction;
}
};

199
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.cpp vendored
View File

@@ -20,6 +20,7 @@ subject to the following restrictions:
#include "CollisionShapes/SphereShape.h" //for raycasting
#include "CollisionShapes/TriangleMeshShape.h" //for raycasting
#include "NarrowPhaseCollision/RaycastCallback.h"
#include "CollisionShapes/CompoundShape.h"
#include "NarrowPhaseCollision/SubSimplexConvexCast.h"
#include "BroadphaseCollision/BroadphaseInterface.h"
@@ -52,67 +53,16 @@ CollisionWorld::~CollisionWorld()
}
void CollisionWorld::UpdateActivationState()
{
m_dispatcher->InitUnionFind(m_collisionObjects.size());
// put the index into m_controllers into m_tag
{
std::vector<CollisionObject*>::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();
}
void CollisionWorld::StoreIslandActivationState()
{
// put the islandId ('find' value) into m_tag
{
UnionFind& unionFind = m_dispatcher->GetUnionFind();
std::vector<CollisionObject*>::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)
void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
{
m_collisionObjects.push_back(collisionObject);
@@ -128,7 +78,9 @@ void CollisionWorld::AddCollisionObject(CollisionObject* collisionObject)
minAabb,
maxAabb,
type,
collisionObject
collisionObject,
collisionFilterGroup,
collisionFilterMask
);
@@ -148,10 +100,13 @@ void CollisionWorld::PerformDiscreteCollisionDetection()
for (size_t i=0;i<m_collisionObjects.size();i++)
{
m_collisionObjects[i]->m_collisionShape->GetAabb(m_collisionObjects[i]->m_worldTransform,aabbMin,aabbMax);
m_broadphase->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
m_pairCache->SetAabb(m_collisionObjects[i]->m_broadphaseHandle,aabbMin,aabbMax);
}
m_broadphase->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);
Dispatcher* dispatcher = GetDispatcher();
if (dispatcher)
dispatcher->DispatchAllCollisionPairs(&m_pairCache->GetOverlappingPair(0),m_pairCache->GetNumOverlappingPairs(),dispatchInfo);
}
@@ -185,58 +140,27 @@ void CollisionWorld::RemoveCollisionObject(CollisionObject* collisionObject)
}
}
void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback)
void RayTestSingle(const SimdTransform& rayFromTrans,const SimdTransform& rayToTrans,
CollisionObject* collisionObject,
const CollisionShape* collisionShape,
const SimdTransform& colObjWorldTransform,
CollisionWorld::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<CollisionObject*>::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())
if (collisionShape->IsConvex())
{
ConvexCast::CastResult castResult;
castResult.m_fraction = 1.f;//??
ConvexShape* convexShape = (ConvexShape*) collisionObject->m_collisionShape;
ConvexShape* convexShape = (ConvexShape*) 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))
if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.length2() > 0.0001f)
@@ -263,12 +187,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
else
{
if (collisionObject->m_collisionShape->IsConcave())
if (collisionShape->IsConcave())
{
TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionObject->m_collisionShape;
TriangleMeshShape* triangleMesh = (TriangleMeshShape*)collisionShape;
SimdTransform worldTocollisionObject = collisionObject->m_worldTransform.inverse();
SimdTransform worldTocollisionObject = colObjWorldTransform.inverse();
SimdVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
SimdVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
@@ -277,12 +201,12 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
struct BridgeTriangleRaycastCallback : public TriangleRaycastCallback
{
RayResultCallback* m_resultCallback;
CollisionWorld::RayResultCallback* m_resultCallback;
CollisionObject* m_collisionObject;
TriangleMeshShape* m_triangleMesh;
BridgeTriangleRaycastCallback( const SimdVector3& from,const SimdVector3& to,
RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh):
CollisionWorld::RayResultCallback* resultCallback, CollisionObject* collisionObject,TriangleMeshShape* triangleMesh):
TriangleRaycastCallback(from,to),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
@@ -321,10 +245,75 @@ void CollisionWorld::RayTest(const SimdVector3& rayFromWorld, const SimdVector3&
triangleMesh->ProcessAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal);
}
} else
{
//todo: use AABB tree or other BVH acceleration structure!
if (collisionShape->IsCompound())
{
const CompoundShape* compoundShape = static_cast<const CompoundShape*>(collisionShape);
int i=0;
for (i=0;i<compoundShape->GetNumChildShapes();i++)
{
SimdTransform childTrans = compoundShape->GetChildTransform(i);
const CollisionShape* childCollisionShape = compoundShape->GetChildShape(i);
SimdTransform childWorldTrans = colObjWorldTransform * childTrans;
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback);
}
}
}
}
}
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);
/// brute force go over all objects. Once there is a broadphase, use that, or
/// add a raycast against aabb first.
std::vector<CollisionObject*>::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))
{
RayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->m_collisionShape,
collisionObject->m_worldTransform,
resultCallback);
}
}

28
extern/bullet/Bullet/CollisionDispatch/CollisionWorld.h vendored
View File

@@ -70,6 +70,7 @@ class BroadphaseInterface;
#include "SimdTransform.h"
#include "CollisionObject.h"
#include "CollisionDispatcher.h" //for definition of CollisionObjectArray
#include "BroadphaseCollision/OverlappingPairCache.h"
#include <vector>
@@ -83,31 +84,36 @@ class CollisionWorld
std::vector<CollisionObject*> m_collisionObjects;
CollisionDispatcher* m_dispatcher;
Dispatcher* m_dispatcher1;
BroadphaseInterface* m_broadphase;
OverlappingPairCache* m_pairCache;
public:
CollisionWorld(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
:m_dispatcher(dispatcher),
m_broadphase(broadphase)
CollisionWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
:m_dispatcher1(dispatcher),
m_pairCache(pairCache)
{
}
virtual ~CollisionWorld();
virtual void UpdateActivationState();
virtual void StoreIslandActivationState();
BroadphaseInterface* GetBroadphase()
{
return m_broadphase;
return m_pairCache;
}
CollisionDispatcher* GetDispatcher()
OverlappingPairCache* GetPairCache()
{
return m_dispatcher;
return m_pairCache;
}
Dispatcher* GetDispatcher()
{
return m_dispatcher1;
}
///LocalShapeInfo gives extra information for complex shapes
@@ -201,7 +207,7 @@ public:
void RayTest(const SimdVector3& rayFromWorld, const SimdVector3& rayToWorld, RayResultCallback& resultCallback);
void AddCollisionObject(CollisionObject* collisionObject);
void AddCollisionObject(CollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1);
CollisionObjectArray& GetCollisionObjectArray()
{

144
extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.cpp vendored Normal file
View File

@@ -0,0 +1,144 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "CollisionDispatch/CompoundCollisionAlgorithm.h"
#include "CollisionDispatch/CollisionObject.h"
#include "CollisionShapes/CompoundShape.h"
CompoundCollisionAlgorithm::CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
:m_dispatcher(ci.m_dispatcher),
m_compoundProxy(*proxy0),
m_otherProxy(*proxy1)
{
CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
assert (colObj->m_collisionShape->IsCompound());
CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
int numChildren = compoundShape->GetNumChildShapes();
m_childProxies.resize( numChildren );
int i;
for (i=0;i<numChildren;i++)
{
m_childProxies[i] = BroadphaseProxy(*proxy0);
}
m_childCollisionAlgorithms.resize(numChildren);
for (i=0;i<numChildren;i++)
{
CollisionShape* childShape = compoundShape->GetChildShape(i);
CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
CollisionShape* orgShape = colObj->m_collisionShape;
colObj->m_collisionShape = childShape;
m_childCollisionAlgorithms[i] = m_dispatcher->FindAlgorithm(m_childProxies[i],m_otherProxy);
colObj->m_collisionShape =orgShape;
}
}
CompoundCollisionAlgorithm::~CompoundCollisionAlgorithm()
{
int numChildren = m_childCollisionAlgorithms.size();
int i;
for (i=0;i<numChildren;i++)
{
delete m_childCollisionAlgorithms[i];
}
}
void CompoundCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
{
CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
assert (colObj->m_collisionShape->IsCompound());
CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
int numChildren = m_childCollisionAlgorithms.size();
int i;
for (i=0;i<numChildren;i++)
{
//temporarily exchange parent CollisionShape with childShape, and recurse
CollisionShape* childShape = compoundShape->GetChildShape(i);
CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
//backup
SimdTransform orgTrans = colObj->m_worldTransform;
CollisionShape* orgShape = colObj->m_collisionShape;
SimdTransform childTrans = compoundShape->GetChildTransform(i);
SimdTransform newChildWorldTrans = orgTrans*childTrans ;
colObj->m_worldTransform = newChildWorldTrans;
colObj->m_collisionShape = childShape;
m_childCollisionAlgorithms[i]->ProcessCollision(&m_childProxies[i],&m_otherProxy,dispatchInfo);
//revert back
colObj->m_collisionShape =orgShape;
colObj->m_worldTransform = orgTrans;
}
}
float CompoundCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
{
CollisionObject* colObj = static_cast<CollisionObject*>(m_compoundProxy.m_clientObject);
assert (colObj->m_collisionShape->IsCompound());
CompoundShape* compoundShape = static_cast<CompoundShape*>(colObj->m_collisionShape);
//We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
//If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
//given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
//determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
//then use each overlapping node AABB against Tree0
//and vise versa.
float hitFraction = 1.f;
int numChildren = m_childCollisionAlgorithms.size();
int i;
for (i=0;i<numChildren;i++)
{
//temporarily exchange parent CollisionShape with childShape, and recurse
CollisionShape* childShape = compoundShape->GetChildShape(i);
CollisionObject* colObj = static_cast<CollisionObject*>(m_childProxies[i].m_clientObject);
//backup
SimdTransform orgTrans = colObj->m_worldTransform;
CollisionShape* orgShape = colObj->m_collisionShape;
SimdTransform childTrans = compoundShape->GetChildTransform(i);
SimdTransform newChildWorldTrans = orgTrans*childTrans ;
colObj->m_worldTransform = newChildWorldTrans;
colObj->m_collisionShape = childShape;
float frac = m_childCollisionAlgorithms[i]->CalculateTimeOfImpact(&m_childProxies[i],&m_otherProxy,dispatchInfo);
if (frac<hitFraction)
{
hitFraction = frac;
}
//revert back
colObj->m_collisionShape =orgShape;
colObj->m_worldTransform = orgTrans;
}
return hitFraction;
}

56
extern/bullet/Bullet/CollisionDispatch/CompoundCollisionAlgorithm.h vendored Normal file
View File

@@ -0,0 +1,56 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef COMPOUND_COLLISION_ALGORITHM_H
#define COMPOUND_COLLISION_ALGORITHM_H
#include "BroadphaseCollision/CollisionAlgorithm.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "BroadphaseCollision/BroadphaseInterface.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
class Dispatcher;
#include "BroadphaseCollision/BroadphaseProxy.h"
#include <vector>
/// CompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
/// Place holder, not fully implemented yet
class CompoundCollisionAlgorithm : public CollisionAlgorithm
{
BroadphaseProxy m_compoundProxy;
BroadphaseProxy m_otherProxy;
std::vector<BroadphaseProxy> m_childProxies;
std::vector<CollisionAlgorithm*> m_childCollisionAlgorithms;
Dispatcher* m_dispatcher;
BroadphaseProxy m_compound;
BroadphaseProxy m_other;
public:
CompoundCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
virtual ~CompoundCollisionAlgorithm();
virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
};
#endif //COMPOUND_COLLISION_ALGORITHM_H

190
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp vendored
View File

@@ -17,18 +17,19 @@ subject to the following restrictions:
#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 "CollisionShapes/ConcaveShape.h"
#include "CollisionDispatch/ManifoldResult.h"
#include "NarrowPhaseCollision/RaycastCallback.h"
#include "CollisionShapes/TriangleMeshShape.h"
#include "CollisionShapes/TriangleShape.h"
#include "CollisionShapes/SphereShape.h"
#include "IDebugDraw.h"
#include "NarrowPhaseCollision/SubSimplexConvexCast.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)
m_ConvexTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
{
}
@@ -39,8 +40,8 @@ ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm()
BoxTriangleCallback::BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
ConvexTriangleCallback::ConvexTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
m_convexProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
m_dispatchInfoPtr(0)
{
@@ -52,7 +53,7 @@ BoxTriangleCallback::BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy
ClearCache();
}
BoxTriangleCallback::~BoxTriangleCallback()
ConvexTriangleCallback::~ConvexTriangleCallback()
{
ClearCache();
m_dispatcher->ReleaseManifold( m_manifoldPtr );
@@ -60,14 +61,14 @@ BoxTriangleCallback::~BoxTriangleCallback()
}
void BoxTriangleCallback::ClearCache()
void ConvexTriangleCallback::ClearCache()
{
m_dispatcher->ClearManifold(m_manifoldPtr);
};
void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int triangleIndex)
{
//just for debugging purposes
@@ -79,22 +80,42 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int
CollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher = m_dispatcher;
CollisionObject* ob = static_cast<CollisionObject*>(m_triangleProxy.m_clientObject);
CollisionObject* colObj = static_cast<CollisionObject*>(m_boxProxy->m_clientObject);
///debug drawing of the overlapping triangles
if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->GetDebugMode() > 0)
{
SimdVector3 color(255,255,0);
SimdTransform& tr = ob->m_worldTransform;
m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[0]),tr(triangle[1]),color);
m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[1]),tr(triangle[2]),color);
m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[2]),tr(triangle[0]),color);
//SimdVector3 center = triangle[0] + triangle[1]+triangle[2];
//center *= 0.333333f;
//m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[0]),tr(center),color);
//m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[1]),tr(center),color);
//m_dispatchInfoPtr->m_debugDraw->DrawLine(tr(triangle[2]),tr(center),color);
}
CollisionObject* colObj = static_cast<CollisionObject*>(m_convexProxy->m_clientObject);
if (colObj->m_collisionShape->IsConvex())
{
TriangleShape tm(triangle[0],triangle[1],triangle[2]);
tm.SetMargin(m_collisionMarginTriangle);
CollisionObject* ob = static_cast<CollisionObject*>(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);
ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexProxy,&m_triangleProxy);
cvxcvxalgo.SetShapeIdentifiers(-1,-1,partId,triangleIndex);
cvxcvxalgo.ProcessCollision(m_convexProxy,&m_triangleProxy,*m_dispatchInfoPtr);
ob->m_collisionShape = tmpShape;
}
@@ -105,22 +126,22 @@ void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle,int partId, int
void BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo)
void ConvexTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo)
{
m_dispatchInfoPtr = &dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
//recalc aabbs
CollisionObject* boxBody = (CollisionObject* )m_boxProxy->m_clientObject;
CollisionObject* convexBody = (CollisionObject* )m_convexProxy->m_clientObject;
CollisionObject* triBody = (CollisionObject* )m_triangleProxy.m_clientObject;
SimdTransform boxInTriangleSpace;
boxInTriangleSpace = triBody->m_worldTransform.inverse() * boxBody->m_worldTransform;
SimdTransform convexInTriangleSpace;
convexInTriangleSpace = triBody->m_worldTransform.inverse() * convexBody->m_worldTransform;
CollisionShape* boxshape = static_cast<CollisionShape*>(boxBody->m_collisionShape);
CollisionShape* convexShape = static_cast<CollisionShape*>(convexBody->m_collisionShape);
//CollisionShape* triangleShape = static_cast<CollisionShape*>(triBody->m_collisionShape);
boxshape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
convexShape->GetAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
float extraMargin = collisionMarginTriangle;//CONVEX_DISTANCE_MARGIN;//+0.1f;
@@ -133,17 +154,17 @@ void BoxTriangleCallback::SetTimeStepAndCounters(float collisionMarginTriangle,c
void ConvexConcaveCollisionAlgorithm::ClearCache()
{
m_boxTriangleCallback.ClearCache();
m_ConvexTriangleCallback.ClearCache();
}
void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
{
CollisionObject* boxBody = static_cast<CollisionObject* >(m_convex.m_clientObject);
CollisionObject* convexBody = static_cast<CollisionObject* >(m_convex.m_clientObject);
CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
if (triBody->m_collisionShape->GetShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
if (triBody->m_collisionShape->IsConcave())
{
if (!m_dispatcher->NeedsCollision(m_convex,m_concave))
@@ -152,19 +173,21 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp
CollisionObject* triOb = static_cast<CollisionObject*>(m_concave.m_clientObject);
TriangleMeshShape* triangleMesh = static_cast<TriangleMeshShape*>( triOb->m_collisionShape);
ConcaveShape* concaveShape = static_cast<ConcaveShape*>( triOb->m_collisionShape);
if (boxBody->m_collisionShape->IsConvex())
if (convexBody->m_collisionShape->IsConvex())
{
float collisionMarginTriangle = triangleMesh->GetMargin();
float collisionMarginTriangle = concaveShape->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);
m_ConvexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle,dispatchInfo);
triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax());
//Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
//m_dispatcher->ClearManifold(m_ConvexTriangleCallback.m_manifoldPtr);
m_ConvexTriangleCallback.m_manifoldPtr->SetBodies(m_convex.m_clientObject,m_concave.m_clientObject);
concaveShape->ProcessAllTriangles( &m_ConvexTriangleCallback,m_ConvexTriangleCallback.GetAabbMin(),m_ConvexTriangleCallback.GetAabbMax());
}
@@ -181,52 +204,101 @@ float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,B
CollisionObject* convexbody = (CollisionObject* )m_convex.m_clientObject;
CollisionObject* triBody = static_cast<CollisionObject* >(m_concave.m_clientObject);
const SimdVector3& from = convexbody->m_worldTransform.getOrigin();
SimdVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
//only perform CCD above a certain treshold, this prevents blocking on the long run
//because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
float squareMot0 = (convexbody->m_interpolationWorldTransform.getOrigin() - convexbody->m_worldTransform.getOrigin()).length2();
if (squareMot0 < convexbody->m_ccdSquareMotionTreshold)
{
return 1.f;
}
//const 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
SimdTransform convexFromLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_worldTransform;
SimdTransform convexToLocal = triBody->m_cachedInvertedWorldTransform * convexbody->m_interpolationWorldTransform;
struct LocalTriangleSphereCastCallback : public TriangleCallback
{
LocalTriangleRaycastCallback(const SimdVector3& from,const SimdVector3& to)
:TriangleRaycastCallback(from,to)
{
SimdTransform m_ccdSphereFromTrans;
SimdTransform m_ccdSphereToTrans;
SimdTransform m_meshTransform;
float m_ccdSphereRadius;
float m_hitFraction;
LocalTriangleSphereCastCallback(const SimdTransform& from,const SimdTransform& to,float ccdSphereRadius,float hitFraction)
:m_ccdSphereFromTrans(from),
m_ccdSphereToTrans(to),
m_ccdSphereRadius(ccdSphereRadius),
m_hitFraction(hitFraction)
{
}
virtual float ReportHit(const SimdVector3& hitNormalLocal, float hitFraction, int partId, int triangleIndex )
virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex)
{
//todo: handle ccd here
return 0.f;
//do a swept sphere for now
SimdTransform ident;
ident.setIdentity();
ConvexCast::CastResult castResult;
castResult.m_fraction = m_hitFraction;
SphereShape pointShape(m_ccdSphereRadius);
TriangleShape triShape(triangle[0],triangle[1],triangle[2]);
VoronoiSimplexSolver simplexSolver;
SubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
//local space?
if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
ident,ident,castResult))
{
if (m_hitFraction > castResult.m_fraction)
m_hitFraction = castResult.m_fraction;
}
}
};
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)
if (triBody->m_collisionShape->IsConcave())
{
SimdVector3 rayAabbMin = convexFromLocal.getOrigin();
rayAabbMin.setMin(convexToLocal.getOrigin());
SimdVector3 rayAabbMax = convexFromLocal.getOrigin();
rayAabbMax.setMax(convexToLocal.getOrigin());
rayAabbMin -= SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
rayAabbMax += SimdVector3(convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius,convexbody->m_ccdSweptShereRadius);
float curHitFraction = 1.f; //is this available?
LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
convexbody->m_ccdSweptShereRadius,curHitFraction);
raycastCallback.m_hitFraction = convexbody->m_hitFraction;
CollisionObject* concavebody = (CollisionObject* )m_concave.m_clientObject;
TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->m_collisionShape;
ConcaveShape* triangleMesh = (ConcaveShape*) concavebody->m_collisionShape;
if (triangleMesh)
{
triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
triangleMesh->ProcessAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
}
}
if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
{
convexbody->m_hitFraction = raycastCallback.m_hitFraction;
return raycastCallback.m_hitFraction;
if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
{
convexbody->m_hitFraction = raycastCallback.m_hitFraction;
return raycastCallback.m_hitFraction;
}
}
return 1.f;

12
extern/bullet/Bullet/CollisionDispatch/ConvexConcaveCollisionAlgorithm.h vendored
View File

@@ -25,10 +25,10 @@ class Dispatcher;
#include "BroadphaseCollision/BroadphaseProxy.h"
class BoxTriangleCallback : public TriangleCallback
///For each triangle in the concave mesh that overlaps with the AABB of a convex (m_convexProxy), ProcessTriangle is called.
class ConvexTriangleCallback : public TriangleCallback
{
BroadphaseProxy* m_boxProxy;
BroadphaseProxy* m_convexProxy;
BroadphaseProxy m_triangleProxy;
SimdVector3 m_aabbMin;
@@ -43,11 +43,11 @@ int m_triangleCount;
PersistentManifold* m_manifoldPtr;
BoxTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
ConvexTriangleCallback(Dispatcher* dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
void SetTimeStepAndCounters(float collisionMarginTriangle,const DispatcherInfo& dispatchInfo);
virtual ~BoxTriangleCallback();
virtual ~ConvexTriangleCallback();
virtual void ProcessTriangle(SimdVector3* triangle, int partId, int triangleIndex);
@@ -75,7 +75,7 @@ class ConvexConcaveCollisionAlgorithm : public CollisionAlgorithm
BroadphaseProxy m_concave;
BoxTriangleCallback m_boxTriangleCallback;
ConvexTriangleCallback m_ConvexTriangleCallback;
public:

121
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.cpp vendored
View File

@@ -233,8 +233,9 @@ public:
};
#endif //USE_HULL
//
// box-box collision algorithm, for simplicity also applies resolution-impulse
// Convex-Convex collision algorithm
//
void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,const DispatcherInfo& dispatchInfo)
{
@@ -314,7 +315,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
//input.m_maximumDistanceSquared = 1e30f;//
// input.m_maximumDistanceSquared = 1e30f;
input.m_transformA = col0->m_worldTransform;
input.m_transformB = col1->m_worldTransform;
@@ -323,15 +324,37 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
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<CollisionObject*>(m_box1.m_clientObject);
CollisionObject* col0 = static_cast<CollisionObject*>(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 point, to simplify things
//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)
@@ -340,48 +363,70 @@ float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
if (!needsCollision)
return 1.f;
CollisionObject* col0 = static_cast<CollisionObject*>(m_box0.m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(m_box1.m_clientObject);
SphereShape sphere(0.f);
ConvexShape* min0 = static_cast<ConvexShape*>(col0->m_collisionShape);
ConvexShape* min1 = static_cast<ConvexShape*>(col1->m_collisionShape);
ConvexCast::CastResult result;
VoronoiSimplexSolver voronoiSimplex;
SubsimplexConvexCast ccd0(&sphere,min1,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1(&sphere,min0,&voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (disableCcd)
return 1.f;
if (ccd1.calcTimeOfImpact(col0->m_worldTransform,col0->m_interpolationWorldTransform,
col1->m_worldTransform,col1->m_interpolationWorldTransform,result))
/// Convex0 against sphere for Convex1
{
//store result.m_fraction in both bodies
if (col0->m_hitFraction > result.m_fraction)
col0->m_hitFraction = result.m_fraction;
ConvexShape* convex0 = static_cast<ConvexShape*>(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;
}
if (col1->m_hitFraction > result.m_fraction)
col1->m_hitFraction = result.m_fraction;
return result.m_fraction;
}
/// Sphere (for convex0) against Convex1
{
ConvexShape* convex1 = static_cast<ConvexShape*>(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 1.f;
return resultFraction;
}

8
extern/bullet/Bullet/CollisionDispatch/ConvexConvexAlgorithm.h vendored
View File

@@ -55,7 +55,13 @@ public:
void SetLowLevelOfDetail(bool useLowLevel);
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{
m_gjkPairDetector.m_partId0=partId0;
m_gjkPairDetector.m_partId1=partId1;
m_gjkPairDetector.m_index0=index0;
m_gjkPairDetector.m_index1=index1;
}
const PersistentManifold* GetManifold()
{

47
extern/bullet/Bullet/CollisionDispatch/ManifoldResult.cpp vendored
View File

@@ -18,6 +18,31 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionDispatch/CollisionObject.h"
///This is to allow MaterialCombiner/Custom Friction/Restitution values
ContactAddedCallback gContactAddedCallback=0;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= CollisionObject::customMaterialCallback;
inline SimdScalar calculateCombinedFriction(const CollisionObject* body0,const CollisionObject* 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;
}
inline SimdScalar calculateCombinedRestitution(const CollisionObject* body0,const CollisionObject* body1)
{
return body0->getRestitution() * body1->getRestitution();
}
ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr)
:m_manifoldPtr(manifoldPtr),
m_body0(body0),
@@ -31,8 +56,12 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S
if (depth > m_manifoldPtr->GetContactBreakingTreshold())
return;
SimdTransform transAInv = m_body0->m_worldTransform.inverse();
SimdTransform transBInv= m_body1->m_worldTransform.inverse();
SimdTransform transAInv = m_body0->m_cachedInvertedWorldTransform;
SimdTransform transBInv= m_body1->m_cachedInvertedWorldTransform;
//transAInv = m_body0->m_worldTransform.inverse();
//transBInv= m_body1->m_worldTransform.inverse();
SimdVector3 pointA = pointInWorld + normalOnBInWorld * depth;
SimdVector3 localA = transAInv(pointA );
SimdVector3 localB = transBInv(pointInWorld);
@@ -52,6 +81,20 @@ void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const S
} else
{
newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1);
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1);
//User can override friction and/or restitution
if (gContactAddedCallback &&
//and if either of the two bodies requires custom material
((m_body0->m_collisionFlags & CollisionObject::customMaterialCallback) ||
(m_body1->m_collisionFlags & CollisionObject::customMaterialCallback)))
{
//experimental feature info, for per-triangle material etc.
(*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
}
m_manifoldPtr->AddManifoldPoint(newPt);
}
}

21
extern/bullet/Bullet/CollisionDispatch/ManifoldResult.h vendored
View File

@@ -20,22 +20,41 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
struct CollisionObject;
class PersistentManifold;
class ManifoldPoint;
typedef bool (*ContactAddedCallback)(ManifoldPoint& cp, const CollisionObject* colObj0,int partId0,int index0,const CollisionObject* colObj1,int partId1,int index1);
extern ContactAddedCallback gContactAddedCallback;
///ManifoldResult is a helper class to manage contact results.
class ManifoldResult : public DiscreteCollisionDetectorInterface::Result
{
PersistentManifold* m_manifoldPtr;
CollisionObject* m_body0;
CollisionObject* m_body1;
int m_partId0;
int m_partId1;
int m_index0;
int m_index1;
public:
ManifoldResult(CollisionObject* body0,CollisionObject* body1,PersistentManifold* manifoldPtr);
virtual ~ManifoldResult() {};
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{
m_partId0=partId0;
m_partId1=partId1;
m_index0=index0;
m_index1=index1;
}
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth);
};
#endif //MANIFOLD_RESULT_H

203
extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.cpp vendored Normal file
View File

@@ -0,0 +1,203 @@
#include "SimulationIslandManager.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionDispatch/CollisionObject.h"
#include "CollisionDispatch/CollisionWorld.h"
SimulationIslandManager::SimulationIslandManager()
{
}
void SimulationIslandManager::InitUnionFind(int n)
{
m_unionFind.reset(n);
}
void SimulationIslandManager::FindUnions(Dispatcher* dispatcher)
{
{
for (int i=0;i<dispatcher->GetNumManifolds();i++)
{
const PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//static objects (invmass 0.f) don't merge !
const CollisionObject* colObj0 = static_cast<const CollisionObject*>(manifold->GetBody0());
const CollisionObject* colObj1 = static_cast<const CollisionObject*>(manifold->GetBody1());
if (colObj0 && colObj1 && dispatcher->NeedsResponse(*colObj0,*colObj1))
{
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
{
m_unionFind.unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
}
void SimulationIslandManager::UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher)
{
InitUnionFind(colWorld->GetCollisionObjectArray().size());
// put the index into m_controllers into m_tag
{
std::vector<CollisionObject*>::iterator i;
int index = 0;
for (i=colWorld->GetCollisionObjectArray().begin();
!(i==colWorld->GetCollisionObjectArray().end()); i++)
{
CollisionObject* collisionObject= (*i);
collisionObject->m_islandTag1 = index;
collisionObject->m_hitFraction = 1.f;
index++;
}
}
// do the union find
FindUnions(dispatcher);
}
void SimulationIslandManager::StoreIslandActivationState(CollisionWorld* colWorld)
{
// put the islandId ('find' value) into m_tag
{
std::vector<CollisionObject*>::iterator i;
int index = 0;
for (i=colWorld->GetCollisionObjectArray().begin();
!(i==colWorld->GetCollisionObjectArray().end()); i++)
{
CollisionObject* collisionObject= (*i);
if (collisionObject->mergesSimulationIslands())
{
collisionObject->m_islandTag1 = m_unionFind.find(index);
} else
{
collisionObject->m_islandTag1 = -1;
}
index++;
}
}
}
//
// todo: this is random access, it can be walked 'cache friendly'!
//
void SimulationIslandManager::BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback)
{
int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++)
{
std::vector<PersistentManifold*> islandmanifold;
//int numSleeping = 0;
bool allSleeping = true;
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if (colObj0->GetActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
for (i=0;i<dispatcher->GetNumManifolds();i++)
{
PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
{
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
{
if (dispatcher->NeedsResponse(*colObj0,*colObj1))
islandmanifold.push_back(manifold);
}
}
}
if (allSleeping)
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
colObj0->SetActivationState( ISLAND_SLEEPING );
}
}
} else
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
{
colObj0->SetActivationState( WANTS_DEACTIVATION);
}
}
}
/// Process the actual simulation, only if not sleeping/deactivated
if (islandmanifold.size())
{
callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
}
}
}
}

58
extern/bullet/Bullet/CollisionDispatch/SimulationIslandManager.h vendored Normal file
View File

@@ -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 SIMULATION_ISLAND_MANAGER_H
#define SIMULATION_ISLAND_MANAGER_H
#include "CollisionDispatch/UnionFind.h"
#include "CollisionCreateFunc.h"
class CollisionWorld;
class Dispatcher;
///SimulationIslandManager creates and handles simulation islands, using UnionFind
class SimulationIslandManager
{
UnionFind m_unionFind;
public:
SimulationIslandManager();
void InitUnionFind(int n);
UnionFind& GetUnionFind() { return m_unionFind;}
virtual void UpdateActivationState(CollisionWorld* colWorld,Dispatcher* dispatcher);
virtual void StoreIslandActivationState(CollisionWorld* world);
void FindUnions(Dispatcher* dispatcher);
struct IslandCallback
{
virtual ~IslandCallback() {};
virtual void ProcessIsland(class PersistentManifold** manifolds,int numManifolds) = 0;
};
void BuildAndProcessIslands(Dispatcher* dispatcher,CollisionObjectArray& collisionObjects, IslandCallback* callback);
};
#endif //SIMULATION_ISLAND_MANAGER_H

12
extern/bullet/Bullet/CollisionShapes/BoxShape.h vendored
View File

@@ -48,24 +48,12 @@ public:
{
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;
}

8
extern/bullet/Bullet/CollisionShapes/CollisionShape.h vendored
View File

@@ -59,9 +59,13 @@ public:
}
bool IsConcave() const
{
return (GetShapeType() > CONCAVE_SHAPES_START_HERE);
return ((GetShapeType() > CONCAVE_SHAPES_START_HERE) &&
(GetShapeType() < CONCAVE_SHAPES_END_HERE));
}
bool IsCompound() const
{
return (GetShapeType() == COMPOUND_SHAPE_PROXYTYPE);
}
virtual void setLocalScaling(const SimdVector3& scaling) =0;
virtual const SimdVector3& getLocalScaling() const =0;

100
extern/bullet/Bullet/CollisionShapes/CompoundShape.cpp vendored Normal file
View File

@@ -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 "CompoundShape.h"
#include "CollisionShape.h"
CompoundShape::CompoundShape()
:m_localAabbMin(1e30f,1e30f,1e30f),
m_localAabbMax(-1e30f,-1e30f,-1e30f),
m_aabbTree(0),
m_collisionMargin(0.f),
m_localScaling(1.f,1.f,1.f)
{
}
CompoundShape::~CompoundShape()
{
}
void CompoundShape::AddChildShape(const SimdTransform& localTransform,CollisionShape* shape)
{
m_childTransforms.push_back(localTransform);
m_childShapes.push_back(shape);
//extend the local aabbMin/aabbMax
SimdVector3 localAabbMin,localAabbMax;
shape->GetAabb(localTransform,localAabbMin,localAabbMax);
for (int i=0;i<3;i++)
{
if (m_localAabbMin[i] > localAabbMin[i])
{
m_localAabbMin[i] = localAabbMin[i];
}
if (m_localAabbMax[i] < localAabbMax[i])
{
m_localAabbMax[i] = localAabbMax[i];
}
}
}
///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void CompoundShape::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 CompoundShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
//approximation: take the inertia from the aabb for now
SimdTransform ident;
ident.setIdentity();
SimdVector3 aabbMin,aabbMax;
GetAabb(ident,aabbMin,aabbMax);
SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
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);
}

117
extern/bullet/Bullet/CollisionShapes/CompoundShape.h vendored Normal file
View File

@@ -0,0 +1,117 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef COMPOUND_SHAPE_H
#define COMPOUND_SHAPE_H
#include "CollisionShape.h"
#include "SimdVector3.h"
#include "SimdTransform.h"
#include "SimdMatrix3x3.h"
#include <vector>
#include "CollisionShapes/CollisionMargin.h"
class OptimizedBvh;
/// CompoundShape allows to store multiple other CollisionShapes
/// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
class CompoundShape : public CollisionShape
{
std::vector<SimdTransform> m_childTransforms;
std::vector<CollisionShape*> m_childShapes;
SimdVector3 m_localAabbMin;
SimdVector3 m_localAabbMax;
OptimizedBvh* m_aabbTree;
public:
CompoundShape();
virtual ~CompoundShape();
void AddChildShape(const SimdTransform& localTransform,CollisionShape* shape);
int GetNumChildShapes() const
{
return m_childShapes.size();
}
CollisionShape* GetChildShape(int index)
{
return m_childShapes[index];
}
const CollisionShape* GetChildShape(int index) const
{
return m_childShapes[index];
}
SimdTransform GetChildTransform(int index)
{
return m_childTransforms[index];
}
const SimdTransform GetChildTransform(int index) const
{
return m_childTransforms[index];
}
///GetAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
void GetAabb(const 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 COMPOUND_SHAPE_PROXYTYPE;}
virtual void SetMargin(float margin)
{
m_collisionMargin = margin;
}
virtual float GetMargin() const
{
return m_collisionMargin;
}
virtual char* GetName()const
{
return "Compound";
}
//this is optional, but should make collision queries faster, by culling non-overlapping nodes
void CreateAabbTreeFromChildren();
const OptimizedBvh* GetAabbTree() const
{
return m_aabbTree;
}
private:
SimdScalar m_collisionMargin;
protected:
SimdVector3 m_localScaling;
};
#endif //COMPOUND_SHAPE_H

28
extern/bullet/Bullet/CollisionShapes/ConcaveShape.cpp vendored Normal file
View File

@@ -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 "ConcaveShape.h"
ConcaveShape::ConcaveShape() : m_collisionMargin(0.f)
{
}
ConcaveShape::~ConcaveShape()
{
}

51
extern/bullet/Bullet/CollisionShapes/ConcaveShape.h vendored Normal file
View File

@@ -0,0 +1,51 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef CONCAVE_SHAPE_H
#define CONCAVE_SHAPE_H
#include "CollisionShapes/CollisionShape.h"
#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
#include "TriangleCallback.h"
///Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB.
///Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface.
class ConcaveShape : public CollisionShape
{
protected:
float m_collisionMargin;
public:
ConcaveShape();
virtual ~ConcaveShape();
virtual void ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const = 0;
virtual float GetMargin() const {
return m_collisionMargin;
}
virtual void SetMargin(float collisionMargin)
{
m_collisionMargin = collisionMargin;
}
};
#endif //CONCAVE_SHAPE_H

96
extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.cpp vendored
View File

@@ -1,96 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ConvexTriangleCallback.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "NarrowPhaseCollision/ManifoldContactAddResult.h"
#include "NarrowPhaseCollision/GjkPairDetector.h"
#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
#include "TriangleShape.h"
//m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
//m_dispatcher->ReleaseManifold( m_manifoldPtr );
ConvexTriangleCallback::ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform)
:m_triangleMeshTransform(triangleMeshTransform),
m_convexTransform(convexTransform),
m_triangleCount(0),
m_convexShape(convexShape),
m_manifoldPtr(manifold)
{
}
ConvexTriangleCallback::~ConvexTriangleCallback()
{
}
void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle)
{
//triangle, convex
TriangleShape tm(triangle[0],triangle[1],triangle[2]);
tm.SetMargin(m_collisionMarginTriangle);
GjkPairDetector::ClosestPointInput input;
input.m_transformA = m_triangleMeshTransform;
input.m_transformB = m_convexTransform;
ManifoldContactAddResult output(m_triangleMeshTransform,m_convexTransform,m_manifoldPtr);
VoronoiSimplexSolver simplexSolver;
MinkowskiPenetrationDepthSolver penetrationDepthSolver;
GjkPairDetector gjkDetector(&tm,m_convexShape,&simplexSolver,&penetrationDepthSolver);
gjkDetector.SetMinkowskiA(&tm);
gjkDetector.SetMinkowskiB(m_convexShape);
input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
input.m_maximumDistanceSquared = 1e30f;//?
gjkDetector.GetClosestPoints(input,output,0);
}
void ConvexTriangleCallback::Update(float collisionMarginTriangle)
{
m_triangleCount = 0;
m_collisionMarginTriangle = collisionMarginTriangle;
SimdTransform boxInTriangleSpace;
boxInTriangleSpace = m_triangleMeshTransform.inverse() * m_convexTransform;
m_convexShape->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
float extraMargin = CONVEX_DISTANCE_MARGIN;
SimdVector3 extra(extraMargin,extraMargin,extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}

64
extern/bullet/Bullet/CollisionShapes/ConvexTriangleCallback.h vendored
View File

@@ -1,64 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, 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_TRIANGLE_CALLBACK_H
#define CONVEX_TRIANGLE_CALLBACK_H
#include "TriangleCallback.h"
class ConvexShape;
class PersistentManifold;
#include "SimdTransform.h"
///ConvexTriangleCallback processes the narrowphase convex-triangle collision detection
class ConvexTriangleCallback: public TriangleCallback
{
SimdVector3 m_aabbMin;
SimdVector3 m_aabbMax ;
SimdTransform m_triangleMeshTransform;
SimdTransform m_convexTransform;
// bool m_useContinuous;
float m_collisionMarginTriangle;
public:
int m_triangleCount;
ConvexShape* m_convexShape;
PersistentManifold* m_manifoldPtr;
ConvexTriangleCallback(PersistentManifold* manifold,ConvexShape* convexShape,const SimdTransform&convexTransform,const SimdTransform& triangleMeshTransform);
void Update(float collisionMarginTriangle);
virtual ~ConvexTriangleCallback();
virtual void ProcessTriangle(SimdVector3* triangle);
inline const SimdVector3& GetAabbMin() const
{
return m_aabbMin;
}
inline const SimdVector3& GetAabbMax() const
{
return m_aabbMax;
}
};
#endif //CONVEX_TRIANGLE_CALLBACK_H

197
extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.cpp vendored Normal file
View File

@@ -0,0 +1,197 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ConvexTriangleMeshShape.h"
#include "CollisionShapes/CollisionMargin.h"
#include "SimdQuaternion.h"
#include "CollisionShapes/StridingMeshInterface.h"
ConvexTriangleMeshShape ::ConvexTriangleMeshShape (StridingMeshInterface* meshInterface)
:m_stridingMesh(meshInterface)
{
}
///It's not nice to have all this virtual function overhead, so perhaps we can also gather the points once
///but then we are duplicating
class LocalSupportVertexCallback: public InternalTriangleIndexCallback
{
SimdVector3 m_supportVertexLocal;
public:
SimdScalar m_maxDot;
SimdVector3 m_supportVecLocal;
LocalSupportVertexCallback(const SimdVector3& supportVecLocal)
: m_supportVertexLocal(0.f,0.f,0.f),
m_supportVecLocal(supportVecLocal),
m_maxDot(-1e30f)
{
}
virtual void InternalProcessTriangleIndex(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 GetSupportVertexLocal()
{
return m_supportVertexLocal;
}
};
SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
SimdVector3 supVec(0.f,0.f,0.f);
SimdScalar newDot,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;
}
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
void ConvexTriangleMeshShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
{
SimdScalar newDot;
//use 'w' component of supportVerticesOut?
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i][3] = -1e30f;
}
}
//todo: could do the batch inside the callback!
for (int j=0;j<numVectors;j++)
{
const SimdVector3& vec = vectors[j];
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supportVerticesOut[j] = supportCallback.GetSupportVertexLocal();
}
}
SimdVector3 ConvexTriangleMeshShape::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 ConvexTriangleMeshShape with the Raytracer Demo
int ConvexTriangleMeshShape::GetNumVertices() const
{
//cache this?
assert(0);
return 0;
}
int ConvexTriangleMeshShape::GetNumEdges() const
{
assert(0);
return 0;
}
void ConvexTriangleMeshShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
{
assert(0);
}
void ConvexTriangleMeshShape::GetVertex(int i,SimdPoint3& vtx) const
{
assert(0);
}
int ConvexTriangleMeshShape::GetNumPlanes() const
{
return 0;
}
void ConvexTriangleMeshShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
{
assert(0);
}
//not yet
bool ConvexTriangleMeshShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
{
assert(0);
return false;
}
void ConvexTriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
{
m_stridingMesh->setScaling(scaling);
}

49
extern/bullet/Bullet/CollisionShapes/ConvexTriangleMeshShape.h vendored Normal file
View File

@@ -0,0 +1,49 @@
#ifndef CONVEX_TRIANGLEMESH_SHAPE_H
#define CONVEX_TRIANGLEMESH_SHAPE_H
#include "PolyhedralConvexShape.h"
#include "BroadphaseCollision/BroadphaseProxy.h" // for the types
#include <vector>
/// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead.
/// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.
class ConvexTriangleMeshShape : public PolyhedralConvexShape
{
class StridingMeshInterface* m_stridingMesh;
public:
ConvexTriangleMeshShape(StridingMeshInterface* meshInterface);
class StridingMeshInterface* GetStridingMesh()
{
return m_stridingMesh;
}
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_TRIANGLEMESH_SHAPE_PROXYTYPE; }
//debugging
virtual char* GetName()const {return "ConvexTrimesh";}
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;
void setLocalScaling(const SimdVector3& scaling);
};
#endif //CONVEX_TRIANGLEMESH_SHAPE_H

31
extern/bullet/Bullet/CollisionShapes/CylinderShape.h vendored
View File

@@ -66,6 +66,17 @@ public:
return CYLINDER_SHAPE_PROXYTYPE;
}
virtual int GetUpAxis() const
{
return 1;
}
//debugging
virtual char* GetName()const
{
return "CylinderY";
}
};
@@ -77,6 +88,16 @@ public:
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
virtual int GetUpAxis() const
{
return 0;
}
//debugging
virtual char* GetName()const
{
return "CylinderX";
}
};
class CylinderShapeZ : public CylinderShape
@@ -87,6 +108,16 @@ public:
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
virtual int GetUpAxis() const
{
return 2;
}
//debugging
virtual char* GetName()const
{
return "CylinderZ";
}
};

18
extern/bullet/Bullet/CollisionShapes/EmptyShape.h vendored
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
#ifndef EMPTY_SHAPE_H
#define EMPTY_SHAPE_H
#include "CollisionShape.h"
#include "ConcaveShape.h"
#include "SimdVector3.h"
#include "SimdTransform.h"
@@ -27,8 +27,9 @@ subject to the following restrictions:
/// EmptyShape is a collision shape without actual collision detection. It can be replaced by another shape during runtime
class EmptyShape : public CollisionShape
/// EmptyShape is a collision shape without actual collision detection.
///It can be replaced by another shape during runtime
class EmptyShape : public ConcaveShape
{
public:
EmptyShape();
@@ -53,22 +54,13 @@ public:
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;

19
extern/bullet/Bullet/CollisionShapes/OptimizedBvh.cpp vendored
View File

@@ -193,21 +193,14 @@ int OptimizedBvh::CalcSplittingAxis(NodeArray& leafNodes,int startIndex,int endI
}
void OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
{
if (aabbMin.length() > 1000.f)
{
for (size_t i=0;i<m_leafNodes.size();i++)
{
const OptimizedBvhNode& node = m_leafNodes[i];
nodeCallback->ProcessNode(&node);
}
} else
{
//WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
}
//either choose recursive traversal (WalkTree) or stackless (WalkStacklessTree)
//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

100
extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.cpp vendored Normal file
View File

@@ -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 "StaticPlaneShape.h"
#include "SimdTransformUtil.h"
StaticPlaneShape::StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant)
:m_planeNormal(planeNormal),
m_planeConstant(planeConstant),
m_localScaling(0.f,0.f,0.f)
{
}
StaticPlaneShape::~StaticPlaneShape()
{
}
void StaticPlaneShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
{
SimdVector3 infvec (1e30f,1e30f,1e30f);
SimdVector3 center = m_planeNormal*m_planeConstant;
aabbMin = center + infvec*m_planeNormal;
aabbMax = aabbMin;
aabbMin.setMin(center - infvec*m_planeNormal);
aabbMax.setMax(center - infvec*m_planeNormal);
aabbMin.setValue(-1e30f,-1e30f,-1e30f);
aabbMax.setValue(1e30f,1e30f,1e30f);
}
void StaticPlaneShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
{
SimdVector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
SimdScalar radius = halfExtents.length();
SimdVector3 center = (aabbMax + aabbMin) * 0.5f;
//this is where the triangles are generated, given AABB and plane equation (normal/constant)
SimdVector3 tangentDir0,tangentDir1;
//tangentDir0/tangentDir1 can be precalculated
SimdPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
SimdVector3 supVertex0,supVertex1;
SimdVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
SimdVector3 triangle[3];
triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
callback->ProcessTriangle(triangle,0,0);
triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
callback->ProcessTriangle(triangle,0,1);
}
void StaticPlaneShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
//moving concave objects not supported
inertia.setValue(0.f,0.f,0.f);
}
void StaticPlaneShape::setLocalScaling(const SimdVector3& scaling)
{
m_localScaling = scaling;
}
const SimdVector3& StaticPlaneShape::getLocalScaling() const
{
return m_localScaling;
}

61
extern/bullet/Bullet/CollisionShapes/StaticPlaneShape.h vendored Normal file
View File

@@ -0,0 +1,61 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef STATIC_PLANE_SHAPE_H
#define STATIC_PLANE_SHAPE_H
#include "CollisionShapes/ConcaveShape.h"
///StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB.
///Assumed is that the other objects is not also infinite, so a reasonable sized AABB.
class StaticPlaneShape : public ConcaveShape
{
protected:
SimdVector3 m_localAabbMin;
SimdVector3 m_localAabbMax;
SimdVector3 m_planeNormal;
SimdVector3 m_localScaling;
SimdScalar m_planeConstant;
public:
StaticPlaneShape(const SimdVector3& planeNormal,SimdScalar planeConstant);
virtual ~StaticPlaneShape();
virtual int GetShapeType() const
{
return STATIC_PLANE_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 "STATICPLANE";}
};
#endif //STATIC_PLANE_SHAPE_H

54
extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.cpp vendored
View File

@@ -15,39 +15,51 @@ subject to the following restrictions:
#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)
TriangleIndexVertexArray::TriangleIndexVertexArray(int numTriangles,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride)
{
IndexedMesh mesh;
mesh.m_numTriangles = numTriangles;
mesh.m_triangleIndexBase = triangleIndexBase;
mesh.m_triangleIndexStride = triangleIndexStride;
mesh.m_numVertices = numVertices;
mesh.m_vertexBase = vertexBase;
mesh.m_vertexStride = vertexStride;
AddIndexedMesh(mesh);
}
void 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;
ASSERT(subpart< getNumSubParts() );
IndexedMesh& mesh = m_indexedMeshes[subpart];
numfaces = m_numTriangleIndices;
(*indexbase) = (unsigned char *)m_triangleIndexBase;
indexstride = m_triangleIndexStride;
numverts = mesh.m_numVertices;
(*vertexbase) = (unsigned char *) mesh.m_vertexBase;
type = PHY_FLOAT;
vertexStride = mesh.m_vertexStride;
numfaces = mesh.m_numTriangles;
(*indexbase) = (unsigned char *)mesh.m_triangleIndexBase;
indexstride = mesh.m_triangleIndexStride;
indicestype = PHY_INTEGER;
}
void 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;
const IndexedMesh& mesh = m_indexedMeshes[subpart];
numfaces = m_numTriangleIndices;
(*indexbase) = (unsigned char *)m_triangleIndexBase;
indexstride = m_triangleIndexStride;
numverts = mesh.m_numVertices;
(*vertexbase) = (const unsigned char *)mesh.m_vertexBase;
type = PHY_FLOAT;
vertexStride = mesh.m_vertexStride;
numfaces = mesh.m_numTriangles;
(*indexbase) = (const unsigned char *)mesh.m_triangleIndexBase;
indexstride = mesh.m_triangleIndexStride;
indicestype = PHY_INTEGER;
}

43
extern/bullet/Bullet/CollisionShapes/TriangleIndexVertexArray.h vendored
View File

@@ -14,22 +14,47 @@ subject to the following restrictions:
*/
#include "StridingMeshInterface.h"
#include <vector>
///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements
///instead of the number of indices, we pass the number of triangles
///todo: explain with pictures
struct IndexedMesh
{
int m_numTriangles;
int* m_triangleIndexBase;
int m_triangleIndexStride;
int m_numVertices;
float* m_vertexBase;
int m_vertexStride;
};
///TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.
///Additional meshes can be added using AddIndexedMesh
///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays.
///So keep those arrays around during the lifetime of this TriangleIndexVertexArray.
class TriangleIndexVertexArray : public StridingMeshInterface
{
std::vector<IndexedMesh> m_indexedMeshes;
int m_numTriangleIndices;
int* m_triangleIndexBase;
int m_triangleIndexStride;
int m_numVertices;
float* m_vertexBase;
int m_vertexStride;
public:
TriangleIndexVertexArray()
{
}
//just to be backwards compatible
TriangleIndexVertexArray(int numTriangleIndices,int* triangleIndexBase,int triangleIndexStride,int numVertices,float* vertexBase,int vertexStride);
void AddIndexedMesh(const IndexedMesh& mesh)
{
m_indexedMeshes.push_back(mesh);
}
virtual void getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0);
virtual void getLockedReadOnlyVertexIndexBase(const unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,const unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart=0) const;
@@ -42,7 +67,9 @@ public:
/// getNumSubParts returns the number of seperate subparts
/// each subpart has a continuous array of vertices and indices
virtual int getNumSubParts() const { return 1;}
virtual int getNumSubParts() const {
return (int)m_indexedMeshes.size();
}
virtual void preallocateVertices(int numverts){}
virtual void preallocateIndices(int numindices){}

3
extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.cpp vendored
View File

@@ -23,8 +23,7 @@ subject to the following restrictions:
#include "stdio.h"
TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
: m_meshInterface(meshInterface),
m_collisionMargin(CONVEX_DISTANCE_MARGIN)
: m_meshInterface(meshInterface)
{
RecalcLocalAabb();
}

24
extern/bullet/Bullet/CollisionShapes/TriangleMeshShape.h vendored
View File

@@ -16,30 +16,24 @@ subject to the following restrictions:
#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"
#include "CollisionShapes/ConcaveShape.h"
#include "CollisionShapes/StridingMeshInterface.h"
///Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
class TriangleMeshShape : public CollisionShape
class TriangleMeshShape : public ConcaveShape
{
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
@@ -68,16 +62,6 @@ public:
//debugging
virtual char* GetName()const {return "TRIANGLEMESH";}
virtual float GetMargin() const {
return m_collisionMargin;
}
virtual void SetMargin(float collisionMargin)
{
m_collisionMargin = collisionMargin;
}
};

3
extern/bullet/Bullet/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h vendored
View File

@@ -34,6 +34,9 @@ struct DiscreteCollisionDetectorInterface
void operator delete(void* ptr) {};
virtual ~Result(){}
///SetShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)=0;
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)=0;
};

5
extern/bullet/Bullet/NarrowPhaseCollision/GjkConvexCast.cpp vendored
View File

@@ -122,12 +122,13 @@ bool GjkConvexCast::calcTimeOfImpact(
n = x - c;
SimdScalar nDotr = n.dot(r);
if (nDotr >= 0.f)
if (nDotr >= -(SIMD_EPSILON*SIMD_EPSILON))
return false;
lambda = lambda - n.dot(n) / nDotr;
if (lambda <= lastLambda)
break;
lastLambda = lambda;
x = s + lambda * r;

64
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.cpp vendored
View File

@@ -18,14 +18,16 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#endif
static const SimdScalar rel_error = SimdScalar(1.0e-5);
SimdScalar rel_error2 = rel_error * rel_error;
float maxdist2 = 1.e30f;
#include <stdio.h>
int gGjkMaxIter=1000;
bool gIrregularCatch = true;
GjkPairDetector::GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(0.f,0.f,1.f),
@@ -33,7 +35,11 @@ m_penetrationDepthSolver(penetrationDepthSolver),
m_simplexSolver(simplexSolver),
m_minkowskiA(objectA),
m_minkowskiB(objectB),
m_ignoreMargin(false)
m_ignoreMargin(false),
m_partId0(-1),
m_index0(-1),
m_partId1(-1),
m_index1(-1)
{
}
@@ -71,7 +77,22 @@ int curIter = 0;
while (true)
{
//rare failure case, perhaps deferate shapes?
if (curIter++ > gGjkMaxIter)
{
#if defined(DEBUG) || defined (_DEBUG)
printf("GjkPairDetector maxIter exceeded:%i\n",curIter);
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(),
squaredDistance,
m_minkowskiA->GetShapeType(),
m_minkowskiB->GetShapeType());
#endif
break;
}
SimdVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
SimdVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
@@ -134,39 +155,6 @@ int curIter = 0;
m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
break;
}
//rare failure case, perhaps deferate shapes?
if (curIter++ > gGjkMaxIter)
{
#define CATCH_ME 1
#ifdef CATCH_ME
//this should not happen, we need to catch it
//#if defined(DEBUG) || defined (_DEBUG)
if (gIrregularCatch)
{
gIrregularCatch = false;
printf("Problem with collision geometry\n");
printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
m_cachedSeparatingAxis.getX(),
m_cachedSeparatingAxis.getY(),
m_cachedSeparatingAxis.getZ(),
squaredDistance,
m_minkowskiA->GetShapeType(),
m_minkowskiB->GetShapeType());
printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
printf("Please include above information, your Platform, version of OS.\n");
printf("Thanks.\n");
}
//#endif
#endif //CATCH_ME
break;
}
}
if (checkSimplex)
@@ -223,6 +211,8 @@ int curIter = 0;
if (isValid)
{
output.SetShapeIdentifiers(m_partId0,m_index0,m_partId1,m_index1);
output.AddContactPoint(
normalInB,
pointOnB,

8
extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h vendored
View File

@@ -39,9 +39,17 @@ class GjkPairDetector : public DiscreteCollisionDetectorInterface
ConvexShape* m_minkowskiA;
ConvexShape* m_minkowskiB;
bool m_ignoreMargin;
public:
//experimental feature information, per triangle, per convex etc.
//'material combiner' / contact added callback
int m_partId0;
int m_index0;
int m_partId1;
int m_index1;
GjkPairDetector(ConvexShape* objectA,ConvexShape* objectB,SimplexSolverInterface* simplexSolver,ConvexPenetrationDepthSolver* penetrationDepthSolver);
virtual ~GjkPairDetector() {};

4
extern/bullet/Bullet/NarrowPhaseCollision/ManifoldPoint.h vendored
View File

@@ -40,6 +40,8 @@ class ManifoldPoint
m_localPointB( pointB ),
m_normalWorldOnB( normal ),
m_distance1( distance ),
m_combinedFriction(0.f),
m_combinedRestitution(0.f),
m_userPersistentData(0),
m_lifeTime(0)
{
@@ -57,6 +59,8 @@ class ManifoldPoint
SimdVector3 m_normalWorldOnB;
float m_distance1;
float m_combinedFriction;
float m_combinedRestitution;
void* m_userPersistentData;

3
extern/bullet/Bullet/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cpp vendored
View File

@@ -32,6 +32,9 @@ struct MyResult : public DiscreteCollisionDetectorInterface::Result
float m_depth;
bool m_hasResult;
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{
}
void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
m_normalOnBInWorld = normalOnBInWorld;

7
extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp vendored
View File

@@ -19,7 +19,8 @@ subject to the following restrictions:
#include <assert.h>
float gContactBreakingTreshold = 0.02f;
ContactDestroyedCallback gContactCallback = 0;
ContactDestroyedCallback gContactDestroyedCallback = 0;
PersistentManifold::PersistentManifold()
@@ -75,9 +76,9 @@ void PersistentManifold::ClearUserCache(ManifoldPoint& pt)
assert(occurance<=0);
#endif //DEBUG_PERSISTENCY
if (pt.m_userPersistentData && gContactCallback)
if (pt.m_userPersistentData && gContactDestroyedCallback)
{
(*gContactCallback)(pt.m_userPersistentData);
(*gContactDestroyedCallback)(pt.m_userPersistentData);
pt.m_userPersistentData = 0;
}

5
extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h vendored
View File

@@ -27,14 +27,15 @@ struct CollisionResult;
extern float gContactBreakingTreshold;
typedef bool (*ContactDestroyedCallback)(void* userPersistentData);
extern ContactDestroyedCallback gContactDestroyedCallback;
extern ContactDestroyedCallback gContactCallback;
#define MANIFOLD_CACHE_SIZE 4
///PersistentManifold maintains contact points, and reduces them to 4
///PersistentManifold maintains contact points, and reduces them to 4.
///It does contact filtering/contact reduction.
class PersistentManifold
{

5
extern/bullet/Bullet/NarrowPhaseCollision/PointCollector.h vendored
View File

@@ -35,6 +35,11 @@ struct PointCollector : public DiscreteCollisionDetectorInterface::Result
{
}
virtual void SetShapeIdentifiers(int partId0,int index0, int partId1,int index1)
{
//??
}
virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
if (depth< m_distance)

5
extern/bullet/Bullet/NarrowPhaseCollision/SimplexSolverInterface.h vendored
View File

@@ -26,8 +26,9 @@ subject to the following restrictions:
#include "VoronoiSimplexSolver.h"
#define SimplexSolverInterface VoronoiSimplexSolver
#else
/// for simplices from 1 to 4 vertices
/// for example Johnson-algorithm or alternative approaches based on
/// 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
{

4
extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.cpp vendored
View File

@@ -37,7 +37,7 @@ bool SubsimplexConvexCast::calcTimeOfImpact(
CastResult& result)
{
MinkowskiSumShape combi(m_convexA,m_convexB);
MinkowskiSumShape combi(m_convexA,m_convexB);
MinkowskiSumShape* convex = &combi;
SimdTransform rayFromLocalA;
@@ -92,7 +92,7 @@ bool SubsimplexConvexCast::calcTimeOfImpact(
{
VdotR = v.dot(r);
if (VdotR >= 0.f)
if (VdotR >= -(SIMD_EPSILON*SIMD_EPSILON))
return false;
else
{

5
extern/bullet/Bullet/NarrowPhaseCollision/SubSimplexConvexCast.h vendored
View File

@@ -22,7 +22,9 @@ subject to the following restrictions:
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;
@@ -34,7 +36,8 @@ 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,

30
extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp vendored
View File

@@ -33,18 +33,7 @@ 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
@@ -128,26 +117,23 @@ float resolveSingleCollision(
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 tau = solverInfo.m_tau;
float Kerp = solverInfo.m_erp;
if (useGlobalSettingContacts)
{
damping = contactDamping;
tau = contactTau;
Kerp = contactTau;
}
float Kcor = tau *Kfps;
float Kcor = Kerp *Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
ConstraintPersistentData* cpd = (ConstraintPersistentData*) contactPoint.m_userPersistentData;
assert(cpd);
SimdScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
@@ -156,7 +142,7 @@ float resolveSingleCollision(
//distance = 0.f;
SimdScalar positionalError = Kcor *-distance;
//jacDiagABInv;
SimdScalar velocityError = cpd->m_restitution - rel_vel * damping;
SimdScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
SimdScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
@@ -187,17 +173,19 @@ float resolveSingleFriction(
)
{
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);
float combinedFriction = cpd->m_friction;
SimdScalar limit = cpd->m_appliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction

2
extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.h vendored
View File

@@ -34,6 +34,7 @@ struct ConstraintPersistentData
m_jacDiagABInv(0.f),
m_persistentLifeTime(0),
m_restitution(0.f),
m_friction(0.f),
m_penetration(0.f)
{
}
@@ -50,6 +51,7 @@ struct ConstraintPersistentData
float m_jacDiagABInvTangent1;
int m_persistentLifeTime;
float m_restitution;
float m_friction;
float m_penetration;
SimdVector3 m_frictionWorldTangential0;
SimdVector3 m_frictionWorldTangential1;

2
extern/bullet/BulletDynamics/ConstraintSolver/ContactSolverInfo.h vendored
View File

@@ -22,7 +22,7 @@ struct ContactSolverInfo
inline ContactSolverInfo()
{
m_tau = 0.4f;
m_tau = 0.6f;
m_damping = 1.0f;
m_friction = 0.3f;
m_restitution = 0.f;

250
extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.cpp vendored Normal file
View File

@@ -0,0 +1,250 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "Generic6DofConstraint.h"
#include "Dynamics/RigidBody.h"
#include "Dynamics/MassProps.h"
#include "SimdTransformUtil.h"
static const SimdScalar kSign[] = { 1.0f, -1.0f, 1.0f };
static const int kAxisA[] = { 1, 0, 0 };
static const int kAxisB[] = { 2, 2, 1 };
Generic6DofConstraint::Generic6DofConstraint()
{
}
Generic6DofConstraint::Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB)
: TypedConstraint(rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB)
{
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//so start all locked
for (int i=0; i<6;++i)
{
m_lowerLimit[i] = 0.0f;
m_upperLimit[i] = 0.0f;
m_accumulatedImpulse[i] = 0.0f;
}
}
void Generic6DofConstraint::BuildJacobian()
{
SimdVector3 normal(0,0,0);
const SimdVector3& pivotInA = m_frameInA.getOrigin();
const SimdVector3& pivotInB = m_frameInB.getOrigin();
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
int i;
//linear part
for (i=0;i<3;i++)
{
if (isLimited(i))
{
normal[i] = 1;
// Create linear atom
new (&m_jacLinear[i]) JacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
// Apply accumulated impulse
SimdVector3 impulse_vector = m_accumulatedImpulse[i] * normal;
m_rbA.applyImpulse( impulse_vector, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
normal[i] = 0;
}
}
// angular part
for (i=0;i<3;i++)
{
if (isLimited(i+3))
{
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
// Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
// Create angular atom
new (&m_jacAng[i]) JacobianEntry(axis,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
// Apply accumulated impulse
SimdVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
}
}
}
void Generic6DofConstraint::SolveConstraint(SimdScalar timeStep)
{
SimdScalar tau = 0.1f;
SimdScalar damping = 1.0f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
SimdVector3 normal(0,0,0);
int i;
// linear
for (i=0;i<3;i++)
{
if (isLimited(i))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
normal[i] = 1;
SimdScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal);
SimdScalar impulse = (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
m_accumulatedImpulse[i] += impulse;
SimdVector3 impulse_vector = normal * impulse;
m_rbA.applyImpulse( impulse_vector, rel_pos1);
m_rbB.applyImpulse(-impulse_vector, rel_pos2);
normal[i] = 0;
}
}
// angular
for (i=0;i<3;i++)
{
if (isLimited(i+3))
{
SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
//velocity error (first order error)
SimdScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
//positional error (zeroth order error)
SimdVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
SimdVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
SimdScalar rel_pos = kSign[i] * axisA.dot(axisB);
//impulse
SimdScalar impulse = -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
m_accumulatedImpulse[i + 3] += impulse;
// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
SimdVector3 axis = kSign[i] * axisA.cross(axisB);
SimdVector3 impulse_vector = axis * impulse;
m_rbA.applyTorqueImpulse( impulse_vector);
m_rbB.applyTorqueImpulse(-impulse_vector);
}
}
}
void Generic6DofConstraint::UpdateRHS(SimdScalar timeStep)
{
}
SimdScalar Generic6DofConstraint::ComputeAngle(int axis) const
{
SimdScalar angle;
switch (axis)
{
case 0:
{
SimdVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
SimdScalar s = v1.dot(w2);
SimdScalar c = v1.dot(v2);
angle = SimdAtan2( s, c );
}
break;
case 1:
{
SimdVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
SimdVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
SimdScalar s = w1.dot(u2);
SimdScalar c = w1.dot(w2);
angle = SimdAtan2( s, c );
}
break;
case 2:
{
SimdVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
SimdVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
SimdVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
SimdScalar s = u1.dot(v2);
SimdScalar c = u1.dot(u2);
angle = SimdAtan2( s, c );
}
break;
}
return angle;
}

114
extern/bullet/BulletDynamics/ConstraintSolver/Generic6DofConstraint.h vendored Normal file
View File

@@ -0,0 +1,114 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GENERIC_6DOF_CONSTRAINT_H
#define GENERIC_6DOF_CONSTRAINT_H
#include "SimdVector3.h"
#include "ConstraintSolver/JacobianEntry.h"
#include "TypedConstraint.h"
class RigidBody;
/// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
/// Work in progress (is still a Hinge actually)
class Generic6DofConstraint : public TypedConstraint
{
JacobianEntry m_jacLinear[3]; // 3 orthogonal linear constraints
JacobianEntry m_jacAng[3]; // 3 orthogonal angular constraints
SimdTransform m_frameInA; // the constraint space w.r.t body A
SimdTransform m_frameInB; // the constraint space w.r.t body B
SimdScalar m_lowerLimit[6]; // the constraint lower limits
SimdScalar m_upperLimit[6]; // the constraint upper limits
SimdScalar m_accumulatedImpulse[6];
public:
Generic6DofConstraint(RigidBody& rbA, RigidBody& rbB, const SimdTransform& frameInA, const SimdTransform& frameInB );
Generic6DofConstraint();
virtual void BuildJacobian();
virtual void SolveConstraint(SimdScalar timeStep);
void UpdateRHS(SimdScalar timeStep);
SimdScalar ComputeAngle(int axis) const;
void setLinearLowerLimit(const SimdVector3& linearLower)
{
m_lowerLimit[0] = linearLower.getX();
m_lowerLimit[1] = linearLower.getY();
m_lowerLimit[2] = linearLower.getZ();
}
void setLinearUpperLimit(const SimdVector3& linearUpper)
{
m_upperLimit[0] = linearUpper.getX();
m_upperLimit[1] = linearUpper.getY();
m_upperLimit[2] = linearUpper.getZ();
}
void setAngularLowerLimit(const SimdVector3& angularLower)
{
m_lowerLimit[3] = angularLower.getX();
m_lowerLimit[4] = angularLower.getY();
m_lowerLimit[5] = angularLower.getZ();
}
void setAngularUpperLimit(const SimdVector3& angularUpper)
{
m_upperLimit[3] = angularUpper.getX();
m_upperLimit[4] = angularUpper.getY();
m_upperLimit[5] = angularUpper.getZ();
}
//first 3 are linear, next 3 are angular
void SetLimit(int axis, SimdScalar lo, SimdScalar hi)
{
m_lowerLimit[axis] = lo;
m_upperLimit[axis] = hi;
}
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//limitIndex: first 3 are linear, next 3 are angular
bool isLimited(int limitIndex)
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
const RigidBody& GetRigidBodyA() const
{
return m_rbA;
}
const RigidBody& GetRigidBodyB() const
{
return m_rbB;
}
};
#endif //GENERIC_6DOF_CONSTRAINT_H

104
extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp vendored
View File

@@ -28,7 +28,7 @@ HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector
SimdVector3& axisInA,SimdVector3& axisInB)
:TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_axisInA(axisInA),
m_axisInB(axisInB),
m_axisInB(-axisInB),
m_angularOnly(false)
{
@@ -73,7 +73,7 @@ void HingeConstraint::BuildJacobian()
//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
//this is unused for now, it's a todo
SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 jointAxis0;
SimdVector3 jointAxis1;
@@ -96,6 +96,102 @@ void HingeConstraint::BuildJacobian()
void HingeConstraint::SolveConstraint(SimdScalar timeStep)
{
//#define NEW_IMPLEMENTATION
#ifdef NEW_IMPLEMENTATION
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
// Dirk: Don't we need to update this after each applied impulse
SimdVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
SimdVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
if (!m_angularOnly)
{
SimdVector3 normal(0,0,0);
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;
// Dirk: Get new angular velocity since it changed after applying an impulse
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
//velocity error (first order error)
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);
SimdScalar impulse = tau*depth/timeStep * jacDiagABInv - 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;
}
}
///solve angular part
// get axes in world space
SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
// constraint axes in world space
SimdVector3 jointAxis0;
SimdVector3 jointAxis1;
SimdPlaneSpace1(axisA,jointAxis0,jointAxis1);
// Dirk: Get new angular velocity since it changed after applying an impulse
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
SimdScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);
float tau1 = tau;//0.f;
SimdScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
SimdVector3 angular_impulse0 = jointAxis0 * impulse0;
m_rbA.applyTorqueImpulse( angular_impulse0);
m_rbB.applyTorqueImpulse(-angular_impulse0);
// Dirk: Get new angular velocity since it changed after applying an impulse
angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
SimdScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
SimdScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
m_rbB.getLinearVelocity(),angvelB);;
SimdScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
SimdVector3 angular_impulse1 = jointAxis1 * impulse1;
m_rbA.applyTorqueImpulse( angular_impulse1);
m_rbB.applyTorqueImpulse(-angular_impulse1);
#else
SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -104,7 +200,7 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
SimdScalar tau = 0.3f;
SimdScalar damping = 1.f;
if (!m_angularOnly)
//linear part
{
for (int i=0;i<3;i++)
{
@@ -169,6 +265,8 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep)
m_rbA.applyTorqueImpulse(-velrel+angularError);
m_rbB.applyTorqueImpulse(velrel-angularError);
#endif
}
void HingeConstraint::UpdateRHS(SimdScalar timeStep)

50
extern/bullet/BulletDynamics/ConstraintSolver/JacobianEntry.h vendored
View File

@@ -42,28 +42,48 @@ public:
const SimdScalar massInvA,
const SimdVector3& inertiaInvB,
const SimdScalar massInvB)
:m_jointAxis(jointAxis)
:m_linearJointAxis(jointAxis)
{
m_aJ = world2A*(rel_pos1.cross(m_jointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_jointAxis));
m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
}
//angular constraint between two different rigidbodies
//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_linearJointAxis(SimdVector3(0.f,0.f,0.f))
{
m_aJ= world2A*m_jointAxis;
m_bJ = world2B*-m_jointAxis;
m_aJ= world2A*jointAxis;
m_bJ = world2B*-jointAxis;
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
}
//angular constraint between two different rigidbodies
JacobianEntry(const SimdVector3& axisInA,
const SimdVector3& axisInB,
const SimdVector3& inertiaInvA,
const SimdVector3& inertiaInvB)
: m_linearJointAxis(SimdVector3(0.f,0.f,0.f))
, m_aJ(axisInA)
, m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
}
//constraint on one rigidbody
@@ -73,13 +93,15 @@ public:
const SimdVector3& jointAxis,
const SimdVector3& inertiaInvA,
const SimdScalar massInvA)
:m_jointAxis(jointAxis)
:m_linearJointAxis(jointAxis)
{
m_aJ= world2A*(rel_pos1.cross(m_jointAxis));
m_bJ = world2A*(rel_pos2.cross(-m_jointAxis));
m_aJ= world2A*(rel_pos1.cross(jointAxis));
m_bJ = world2A*(rel_pos2.cross(-jointAxis));
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = SimdVector3(0.f,0.f,0.f);
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
ASSERT(m_Adiag > 0.0f);
}
SimdScalar getDiagonal() const { return m_Adiag; }
@@ -88,7 +110,7 @@ public:
SimdScalar getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
{
const JacobianEntry& jacA = *this;
SimdScalar lin = massInvA * jacA.m_jointAxis.dot(jacB.m_jointAxis);
SimdScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
SimdScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
@@ -99,7 +121,7 @@ public:
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 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
SimdVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
SimdVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
SimdVector3 lin0 = massInvA * lin ;
@@ -113,7 +135,7 @@ public:
SimdVector3 linrel = linvelA - linvelB;
SimdVector3 angvela = angvelA * m_aJ;
SimdVector3 angvelb = angvelB * m_bJ;
linrel *= m_jointAxis;
linrel *= m_linearJointAxis;
angvela += angvelb;
angvela += linrel;
SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
@@ -121,7 +143,7 @@ public:
}
//private:
SimdVector3 m_jointAxis;
SimdVector3 m_linearJointAxis;
SimdVector3 m_aJ;
SimdVector3 m_bJ;
SimdVector3 m_0MinvJt;

265
extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp vendored
View File

@@ -1,265 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, 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 <math.h>
#include <float.h>//FLT_MAX
#ifdef WIN32
#include <memory.h>
#endif
#include <string.h>
#include <stdio.h>
#if defined (WIN32)
#include <malloc.h>
#else
#if defined (__FreeBSD__) || defined (__OpenBSD__)
#include <stdlib.h>
#else
#include <alloca.h>
#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;j<numManifolds;j++)
{
int body0=-1,body1=-1;
PersistentManifold* manifold = manifoldPtr[j];
if (manifold->GetNumContacts() > 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;i<numBodies;i++)
{
if (bodies[i] == body)
return i;
}
//if not found, create a new body
bodies[numBodies++] = body;
//convert data
body->m_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;i<numContacts;i++)
{
if (debugDrawer)
{
const ManifoldPoint& cp = manifold->GetContactPoint(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
};

66
extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h vendored
View File

@@ -1,66 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, 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

20
extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp → extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp vendored
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
#include "SimpleConstraintSolver.h"
#include "SequentialImpulseConstraintSolver.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "Dynamics/RigidBody.h"
#include "ContactConstraint.h"
@@ -45,14 +45,14 @@ bool MyContactDestroyedCallback(void* userPersistentData)
}
SimpleConstraintSolver::SimpleConstraintSolver()
SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
{
gContactCallback = &MyContactDestroyedCallback;
gContactDestroyedCallback = &MyContactDestroyedCallback;
}
/// SimpleConstraintSolver Sequentially applies impulses
float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
/// SequentialImpulseConstraintSolver Sequentially applies impulses
float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
{
ContactSolverInfo info = infoGlobal;
@@ -113,7 +113,7 @@ SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
{
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
@@ -188,10 +188,10 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
SimdScalar rel_vel;
rel_vel = cp.m_normalWorldOnB.dot(vel);
float combinedRestitution = body0->getRestitution() * body1->getRestitution();
float combinedRestitution = cp.m_combinedRestitution;
cpd->m_penetration = cp.GetDistance();
cpd->m_friction = cp.m_combinedFriction;
cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
if (cpd->m_restitution <= 0.) //0.f)
{
@@ -294,7 +294,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta
return maxImpulse;
}
float SimpleConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
{
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();

14
extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h → extern/bullet/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h vendored
View File

@@ -13,17 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SIMPLE_CONSTRAINT_SOLVER_H
#define SIMPLE_CONSTRAINT_SOLVER_H
#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#include "ConstraintSolver.h"
class IDebugDraw;
/// SimpleConstraintSolver uses a Propagation Method and Sequentially applies impulses
/// SequentialImpulseConstraintSolver 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
class SequentialImpulseConstraintSolver : public ConstraintSolver
{
float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer);
@@ -31,13 +31,13 @@ class SimpleConstraintSolver : public ConstraintSolver
public:
SimpleConstraintSolver();
SequentialImpulseConstraintSolver();
virtual ~SimpleConstraintSolver() {}
virtual ~SequentialImpulseConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0);
};
#endif //SIMPLE_CONSTRAINT_SOLVER_H
#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

849
extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp vendored
View File

@@ -1,849 +0,0 @@
/*************************************************************************
* *
* 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 <math.h>
#include <float.h>//FLT_MAX
#ifdef WIN32
#include <memory.h>
#endif
#include <string.h>
#include <stdio.h>
#if defined (WIN32)
#include <malloc.h>
#else
#if defined (__FreeBSD__) || defined (__OpenBSD__)
#include <stdlib.h>
#else
#include <alloca.h>
#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; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
SimdScalar k = body[b1]->getInvMass();
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<m; i++) {
int b1 = jb[i*2];
dRealMutablePtr out_ptr = out + b1*6;
if ((b1 == onlyBody1) || (b1 == onlyBody2))
{
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i] ;
}
iMJ_ptr += 6;
int b2 = jb[i*2+1];
if ((b2 == onlyBody1) || (b2 == onlyBody2))
{
if (b2 >= 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<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
dRealMutablePtr out_ptr = out + b1*6;
for (j=0; j<6; j++)
out_ptr[j] += iMJ_ptr[j] * in[i];
iMJ_ptr += 6;
if (b2 >= 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<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
SimdScalar sum = 0;
dRealMutablePtr in_ptr = in + b1*6;
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
J_ptr += 6;
if (b2 >= 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<m; i++) lambda[i] *= 0.9;
#else
dSetZero1 (lambda,m);
#endif
// the lambda computed at the previous iteration.
// this is used to measure error for when we are reordering the indexes.
dRealAllocaArray (last_lambda,m);
// a copy of the 'hi' vector in case findex[] is being used
dRealAllocaArray (hicopy,m);
memcpy (hicopy,hi,m*sizeof(float));
// precompute iMJ = inv(M)*J'
dRealAllocaArray (iMJ,m*12);
compute_invM_JT (m,J,iMJ,jb,body,invI);
// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
// as we change lambda.
#ifdef WARM_STARTING
multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#else
dSetZero1 (invMforce,nb*6);
#endif
// precompute 1 / diagonals of A
dRealAllocaArray (Ad,m);
dRealPtr iMJ_ptr = iMJ;
dRealMutablePtr J_ptr = J;
for (i=0; i<m; i++) {
float sum = 0;
for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
if (jb[i*2+1] >= 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<m; i++) {
for (j=0; j<12; j++) {
J_ptr[0] *= Ad[i];
J_ptr++;
}
rhs[i] *= Ad[i];
}
// scale Ad by CFM
for (i=0; i<m; i++)
Ad[i] *= cfm[i];
// order to solve constraint rows in
IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
j=0;
for (i=0; i<m; i++)
if (findex[i] < 0)
order[j++].index = i;
for (i=0; i<m; i++)
if (findex[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<m; i++) {
order[i].error = i;
order[i].findex = findex[i];
order[i].index = i;
}
}
else {
// sort the constraints so that the ones converging slowest
// get solved last. use the absolute (not relative) error.
for (i=0; i<m; i++) {
float v1 = dFabs (lambda[i]);
float v2 = dFabs (last_lambda[i]);
float max = (v1 > 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<m; ++i) {
IndexError tmp = order[i];
int swapi = dRandInt2(i+1);
order[i] = order[swapi];
order[swapi] = tmp;
}
}
#endif
//@@@ potential optimization: swap lambda and last_lambda pointers rather
// than copying the data. we must make sure lambda is properly
// returned to the caller
memcpy (last_lambda,lambda,m*sizeof(float));
for (int i=0; i<m; i++) {
// @@@ potential optimization: we could pre-sort J and iMJ, thereby
// linearizing access to those arrays. hmmm, this does not seem
// like a win, but we should think carefully about our memory
// access pattern.
int index = order[i].index;
J_ptr = J + index*12;
iMJ_ptr = iMJ + index*12;
// set the limits for this constraint. note that 'hicopy' is used.
// this is the place where the QuickStep method differs from the
// direct LCP solving method, since that method only performs this
// limit adjustment once per time step, whereas this method performs
// once per iteration per constraint row.
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 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; i<nb; i++)
body[i]->m_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; i<nb; i++) {
dMatrix3 tmp;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_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; i<nb; i++) {
dMatrix3 tmp;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->m_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; j<nj; j++) { // i=dest, j=src
joint[j]->GetInfo1 (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<nj; i++) {
ofs[i] = m;
m += info[i].m;
}
// if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12);
int *jb = (int*) alloca (m*2*sizeof(int));
if (m > 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; i<m; i++) findex[i] = -1;
// get jacobian data from constraints. an m*12 matrix will be created
// to store the two jacobian blocks from each constraint. it has this
// format:
//
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ .
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows)
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
// l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
// etc...
//
// (lll) = linear jacobian data
// (aaa) = angular jacobian data
//
dSetZero1 (J,m*12);
BU_Joint::Info2 Jinfo;
Jinfo.rowskip = 12;
Jinfo.fps = stepsize1;
Jinfo.erp = global_erp;
for (i=0; i<nj; i++) {
Jinfo.J1l = J + ofs[i]*12;
Jinfo.J1a = Jinfo.J1l + 3;
Jinfo.J2l = Jinfo.J1l + 6;
Jinfo.J2a = Jinfo.J1l + 9;
Jinfo.c = c + ofs[i];
Jinfo.cfm = cfm + ofs[i];
Jinfo.lo = lo + ofs[i];
Jinfo.hi = hi + ofs[i];
Jinfo.findex = findex + ofs[i];
joint[i]->GetInfo2 (&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<info[i].m; j++) {
if (findex[ofs[i] + 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; i<nj; i++) {
int b1 = (joint[i]->node[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; j<info[i].m; j++) {
jb_ptr[0] = b1;
jb_ptr[1] = b2;
jb_ptr += 2;
}
}
dIASSERT (jb_ptr == jb+2*m);
// compute the right hand side `rhs'
dRealAllocaArray (tmp1,nb*6);
// put v/h + invM*fe into tmp1
for (i=0; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
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; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
// scale CFM
for (i=0; i<m; i++)
cfm[i] *= stepsize1;
// load lambda from the value saved on the previous iteration
dRealAllocaArray (lambda,m);
#ifdef WARM_STARTING
dSetZero1 (lambda,m); //@@@ shouldn't be necessary
for (i=0; i<nj; i++) {
memcpy (lambda+ofs[i],joint[i]->lambda,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; i<nj; i++) {
memcpy (joint[i]->lambda,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; i<nb; i++) {
SimdVector3 linvel = body[i]->getLinearVelocity();
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; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
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

45
extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.h vendored
View File

@@ -1,45 +0,0 @@
/*************************************************************************
* *
* 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

2
extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp vendored
View File

@@ -178,7 +178,7 @@ void ContactJoint::GetInfo2(Info2 *info)
c2[1] = relativePositionB[1];
c2[2] = relativePositionB[2];
//combined friction is available in the contact point
float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
// first friction direction

7
extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp vendored
View File

@@ -32,10 +32,13 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc
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)
{
//moved to CollisionObject
m_friction = friction;
m_restitution = restitution;
m_debugBodyId = uniqueId++;
setMassProps(massProps.m_mass, massProps.m_inertiaLocal);

26
extern/bullet/BulletDynamics/Dynamics/RigidBody.h vendored
View File

@@ -31,7 +31,6 @@ typedef SimdScalar dMatrix3[4*3];
extern float gLinearAirDamping;
extern bool gUseEpa;
#define MAX_RIGIDBODIES 8192
/// RigidBody class for RigidBody Dynamics
@@ -173,22 +172,7 @@ public:
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
@@ -228,8 +212,6 @@ private:
SimdScalar m_angularDamping;
SimdScalar m_inverseMass;
SimdScalar m_friction;
SimdScalar m_restitution;
SimdScalar m_kinematicTimeStep;
@@ -254,6 +236,7 @@ public:
m_broadphaseProxy = broadphaseProxy;
}
/// for ode solver-binding
dMatrix3 m_R;//temp
@@ -261,10 +244,11 @@ public:
dMatrix3 m_invI;
int m_odeTag;
float m_padding[1024];
SimdVector3 m_tacc;//temp
SimdVector3 m_facc;
int m_debugBodyId;
};

31
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp vendored
View File

@@ -34,7 +34,7 @@ class BP_Proxy;
float gDeactivationTime = 2.f;
bool gDisableDeactivation = false;
float gLinearSleepingTreshold = 0.4f;
float gLinearSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.0f;
#include "Dynamics/MassProps.h"
@@ -210,28 +210,6 @@ void CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
}
void CcdPhysicsController::SetMargin(float margin)
{
if (m_body && m_body->GetCollisionShape())
{
m_body->GetCollisionShape()->SetMargin(margin);
}
}
float CcdPhysicsController::GetMargin() const
{
if (m_body && m_body->GetCollisionShape())
{
return m_body->GetCollisionShape()->GetMargin();
}
return 0.f;
}
// kinematic methods
void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
{
@@ -557,6 +535,7 @@ PHY_IPhysicsController* CcdPhysicsController::GetReplica()
DefaultMotionState::DefaultMotionState()
{
m_worldTransform.setIdentity();
m_localScaling.setValue(1.f,1.f,1.f);
}
@@ -574,9 +553,9 @@ void DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
void DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
{
scaleX = 1.;
scaleY = 1.;
scaleZ = 1.;
scaleX = m_localScaling.getX();
scaleY = m_localScaling.getY();
scaleZ = m_localScaling.getZ();
}
void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)

49
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h vendored
View File

@@ -39,20 +39,38 @@ extern bool gDisableDeactivation;
class CcdPhysicsEnvironment;
struct CcdConstructionInfo
{
///CollisionFilterGroups provides some optional usage of basic collision filtering
///this is done during broadphase, so very early in the pipeline
///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision
enum CollisionFilterGroups
{
DefaultFilter = 1,
StaticFilter = 2,
KinematicFilter = 4,
DebrisFilter = 8,
AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter,
};
CcdConstructionInfo()
: m_gravity(0,0,0),
m_scaling(1.f,1.f,1.f),
m_mass(0.f),
m_restitution(0.1f),
m_friction(0.5f),
m_linearDamping(0.1f),
m_angularDamping(0.1f),
m_collisionFlags(0),
m_collisionFilterGroup(DefaultFilter),
m_collisionFilterMask(AllFilter),
m_MotionState(0),
m_physicsEnv(0),
m_inertiaFactor(1.f),
m_scaling(1.f,1.f,1.f),
m_collisionFlags(0)
m_inertiaFactor(1.f)
{
}
@@ -66,6 +84,15 @@ struct CcdConstructionInfo
SimdScalar m_angularDamping;
int m_collisionFlags;
///optional use of collision group/mask:
///only collision with object goups that match the collision mask.
///this is very basic early out. advanced collision filtering should be
///done in the CollisionDispatcher::NeedsCollision and NeedsResponse
///both values default to 1
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
CollisionShape* m_collisionShape;
class PHY_IMotionState* m_MotionState;
@@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController
virtual void setNewClientInfo(void* clientinfo);
virtual PHY_IPhysicsController* GetReplica();
///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
short int GetCollisionFilterGroup() const
{
return m_cci.m_collisionFilterGroup;
}
///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
short int GetCollisionFilterMask() const
{
return m_cci.m_collisionFilterMask;
}
virtual void calcXform() {} ;
virtual void SetMargin(float margin);
virtual float GetMargin() const;
virtual void SetMargin(float margin) {};
virtual float GetMargin() const {return 0.f;};
bool wantsSleeping();
@@ -208,6 +246,7 @@ class DefaultMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations();
SimdTransform m_worldTransform;
SimdVector3 m_localScaling;
};

253
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp vendored
View File

@@ -30,14 +30,12 @@ subject to the following restrictions:
#include "CollisionShapes/ConvexShape.h"
#include "CollisionShapes/ConeShape.h"
#include "CollisionDispatch/SimulationIslandManager.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionShapes/TriangleMeshShape.h"
#include "ConstraintSolver/OdeConstraintSolver.h"
#include "ConstraintSolver/SimpleConstraintSolver.h"
#include "ConstraintSolver/SequentialImpulseConstraintSolver.h"
//profiling/timings
@@ -56,7 +54,7 @@ subject to the following restrictions:
#include "PHY_IMotionState.h"
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
#include "CollisionDispatch/UnionFind.h"
#include "CollisionShapes/SphereShape.h"
@@ -77,6 +75,7 @@ RaycastVehicle::VehicleTuning gTuning;
#include "ConstraintSolver/ConstraintSolver.h"
#include "ConstraintSolver/Point2PointConstraint.h"
#include "ConstraintSolver/HingeConstraint.h"
#include "ConstraintSolver/Generic6DofConstraint.h"
//#include "BroadphaseCollision/QueryDispatcher.h"
@@ -322,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
:m_scalingPropagated(false),
m_numIterations(10),
m_numTimeSubSteps(1),
@@ -340,14 +339,14 @@ m_enableSatCollisionDetection(false)
dispatcher = new CollisionDispatcher();
if(!broadphase)
if(!pairCache)
{
//todo: calculate/let user specify this world sizes
SimdVector3 worldMin(-10000,-10000,-10000);
SimdVector3 worldMax(10000,10000,10000);
broadphase = new AxisSweep3(worldMin,worldMax);
pairCache = new AxisSweep3(worldMin,worldMax);
//broadphase = new SimpleBroadphase();
}
@@ -355,11 +354,13 @@ m_enableSatCollisionDetection(false)
setSolverType(1);//issues with quickstep and memory allocations
m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
m_collisionWorld = new CollisionWorld(dispatcher,pairCache);
m_debugDrawer = 0;
m_gravity = SimdVector3(0.f,-10.f,0.f);
m_islandManager = new SimulationIslandManager();
}
@@ -373,7 +374,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
body->setGravity( m_gravity );
m_controllers.push_back(ctrl);
m_collisionWorld->AddCollisionObject(body);
m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
assert(body->m_broadphaseHandle);
@@ -385,7 +386,8 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
assert(shapeinterface);
const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
SimdPoint3 minAabb,maxAabb;
@@ -637,6 +639,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive())
{
if (!body->IsStatic())
@@ -654,7 +659,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
BroadphaseInterface* scene = GetBroadphase();
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
//
@@ -674,8 +679,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = this->m_debugDrawer;
scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
scene->RefreshOverlappingPairs();
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
#ifdef USE_QUICKPROF
@@ -685,7 +692,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
int numRigidBodies = m_controllers.size();
m_collisionWorld->UpdateActivationState();
m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
{
int i;
@@ -702,15 +710,15 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
if (colObj0->IsActive() || colObj1->IsActive())
{
GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
m_collisionWorld->StoreIslandActivationState();
m_islandManager->StoreIslandActivationState(GetCollisionWorld());
//contacts
@@ -762,7 +770,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#endif //NEW_BULLET_VEHICLE_SUPPORT
struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback
{
ContactSolverInfo& m_solverInfo;
@@ -803,7 +811,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#endif //USE_QUICKPROF
/// solve all the contact points and contact friction
GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildAndProcessIslands");
@@ -842,7 +850,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
//pairCache->RefreshOverlappingPairs();//??
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
toi = dispatchInfo.m_timeOfImpact;
}
@@ -871,6 +881,11 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
if (!body->IsStatic())
{
if (body->m_hitFraction < 1.f)
{
//set velocity to zero... until we have proper CCD integrated
body->setLinearVelocity(body->getLinearVelocity()*0.5f);
}
body->predictIntegratedTransform(timeStep* toi, predictedTrans);
body->proceedToTransform( predictedTrans);
}
@@ -1033,7 +1048,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType)
if (m_solverType != solverType)
{
m_solver = new SimpleConstraintSolver();
m_solver = new SequentialImpulseConstraintSolver();
break;
}
@@ -1043,7 +1058,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType)
default:
if (m_solverType != solverType)
{
m_solver = new OdeConstraintSolver();
// m_solver = new OdeConstraintSolver();
break;
}
@@ -1182,8 +1197,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * -axisInA;
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
@@ -1213,9 +1228,55 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
break;
}
case PHY_GENERIC_6DOF_CONSTRAINT:
{
Generic6DofConstraint* genericConstraint = 0;
if (rb1)
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
} else
{
// TODO: Implement single body case...
}
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
break;
}
case PHY_ANGULAR_CONSTRAINT:
angularOnly = true;
case PHY_LINEHINGE_CONSTRAINT:
{
HingeConstraint* hinge = 0;
@@ -1271,21 +1332,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
//Following the COLLADA physics specification for constraints
int CcdPhysicsEnvironment::createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& frameInA,
SimdTransform& frameInB,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
)
{
//we could either add some logic to recognize ball-socket and hinge, or let that up to the user
//perhaps some warning or hint that hinge/ball-socket is more efficient?
Generic6DofConstraint* genericConstraint = 0;
CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
RigidBody* rb0 = ctrl0->GetRigidBody();
RigidBody* rb1 = ctrl1->GetRigidBody();
if (rb1)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
genericConstraint->setLinearLowerLimit(linearMinLimits);
genericConstraint->setLinearUpperLimit(linearMaxLimits);
genericConstraint->setAngularLowerLimit(angularMinLimits);
genericConstraint->setAngularUpperLimit(angularMaxLimits);
} else
{
// TODO: Implement single body case...
//No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
}
return 0.f;
if (genericConstraint)
{
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
}
return 0;
}
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
{
std::vector<TypedConstraint*>::iterator i;
@@ -1296,15 +1398,7 @@ void CcdPhysicsEnvironment::removeConstraint(int constraintId)
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintId)
{
//activate objects
if (constraint->GetRigidBodyA().mergesSimulationIslands())
constraint->GetRigidBodyA().activate();
if (constraint->GetRigidBodyB().mergesSimulationIslands())
constraint->GetRigidBodyB().activate();
std::swap(*i, m_constraints.back());
m_constraints.pop_back();
break;
}
@@ -1352,13 +1446,6 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
SimdVector3 rayFrom(fromX,fromY,fromZ);
SimdVector3 rayTo(toX,toY,toZ);
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 color (1,0,0);
m_debugDrawer->DrawLine(rayFrom,rayTo,color);
}
SimdVector3 hitPointWorld,normalWorld;
//Either Ray Cast with or without filtering
@@ -1376,26 +1463,10 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
hitX = rayCallback.m_hitPointWorld.getX();
hitY = rayCallback.m_hitPointWorld.getY();
hitZ = rayCallback.m_hitPointWorld.getZ();
if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON)
{
rayCallback.m_hitNormalWorld.normalize();
}
normalX = rayCallback.m_hitNormalWorld.getX();
normalY = rayCallback.m_hitNormalWorld.getY();
normalZ = rayCallback.m_hitNormalWorld.getZ();
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 colorNormal(0,0,1);
m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal);
SimdVector3 color (0,1,0);
m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color);
}
}
@@ -1425,15 +1496,6 @@ BroadphaseInterface* CcdPhysicsEnvironment::GetBroadphase()
const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
{
return m_collisionWorld->GetDispatcher();
}
CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
{
return m_collisionWorld->GetDispatcher();
}
CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
{
@@ -1448,6 +1510,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
//first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
//delete m_dispatcher;
delete m_collisionWorld;
delete m_islandManager;
}
@@ -1464,15 +1528,9 @@ CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
}
int CcdPhysicsEnvironment::GetNumManifolds() const
{
return GetDispatcher()->GetNumManifolds();
}
const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const
{
return GetDispatcher()->GetManifoldByIndexInternal(index);
}
TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId)
{
@@ -1565,9 +1623,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr
void CcdPhysicsEnvironment::CallbackTriggers()
{
CcdPhysicsController* ctrl0=0,*ctrl1=0;
if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)))
{
//walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
@@ -1577,6 +1636,16 @@ void CcdPhysicsEnvironment::CallbackTriggers()
int numContacts = manifold->GetNumContacts();
if (numContacts)
{
if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))
{
for (int j=0;j<numContacts;j++)
{
SimdVector3 color(1,0,0);
const ManifoldPoint& cp = manifold->GetContactPoint(j);
if (m_debugDrawer)
m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
}
}
RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
@@ -1603,6 +1672,7 @@ void CcdPhysicsEnvironment::CallbackTriggers()
}
}
@@ -1764,3 +1834,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera
return sphereController;
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
}
return 0.f;
}

68
extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h vendored
View File

@@ -20,11 +20,13 @@ subject to the following restrictions:
#include <vector>
class CcdPhysicsController;
#include "SimdVector3.h"
#include "SimdTransform.h"
class TypedConstraint;
class SimulationIslandManager;
class CollisionDispatcher;
class Dispatcher;
//#include "BroadphaseInterface.h"
@@ -37,6 +39,7 @@ class Dispatcher;
class WrapperVehicle;
class PersistentManifold;
class BroadphaseInterface;
class OverlappingPairCache;
class IDebugDraw;
/// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection.
@@ -47,12 +50,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
{
SimdVector3 m_gravity;
protected:
IDebugDraw* m_debugDrawer;
//solver iterations
int m_numIterations;
//timestep subdivisions
int m_numTimeSubSteps;
int m_ccdMode;
int m_solverType;
int m_profileTimings;
@@ -60,9 +68,10 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
ContactSolverInfo m_solverInfo;
SimulationIslandManager* m_islandManager;
public:
CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
virtual ~CcdPhysicsEnvironment();
@@ -98,7 +107,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
virtual void endFrame() {};
/// Perform an integration step of duration 'timeStep'.
virtual bool proceedDeltaTime(double curTime,float timeStep);
bool proceedDeltaTimeOneStep(float timeStep);
virtual bool proceedDeltaTimeOneStep(float timeStep);
virtual void setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){};
//returns 0.f if no fixed timestep is used
@@ -112,7 +121,22 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ);
virtual void removeConstraint(int constraintid);
//Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
);
virtual void `straint(int constraintid);
virtual float getAppliedImpulse(int constraintid);
@@ -160,9 +184,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
BroadphaseInterface* GetBroadphase();
CollisionDispatcher* GetDispatcher();
const CollisionDispatcher* GetDispatcher() const;
bool IsSatCollisionDetectionEnabled() const
{
@@ -180,16 +204,39 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
CcdPhysicsController* GetPhysicsController( int index);
int GetNumManifolds() const;
const PersistentManifold* GetManifold(int index) const;
std::vector<TypedConstraint*> m_constraints;
private:
void SyncMotionStates(float timeStep);
class CollisionWorld* GetCollisionWorld()
{
return m_collisionWorld;
}
const class CollisionWorld* GetCollisionWorld() const
{
return m_collisionWorld;
}
SimulationIslandManager* GetSimulationIslandManager()
{
return m_islandManager;
}
const SimulationIslandManager* GetSimulationIslandManager() const
{
return m_islandManager;
}
protected:
std::vector<CcdPhysicsController*> m_controllers;
@@ -206,6 +253,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
bool m_scalingPropagated;
};

349
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp vendored Normal file
View File

@@ -0,0 +1,349 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ParallelIslandDispatcher.h"
#include "BroadphaseCollision/CollisionAlgorithm.h"
#include "CollisionDispatch/ConvexConvexAlgorithm.h"
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
#include "CollisionDispatch/ConvexConcaveCollisionAlgorithm.h"
#include "CollisionDispatch/CompoundCollisionAlgorithm.h"
#include "CollisionShapes/CollisionShape.h"
#include "CollisionDispatch/CollisionObject.h"
#include <algorithm>
static int gNumManifold2 = 0;
ParallelIslandDispatcher::ParallelIslandDispatcher ():
m_useIslands(true),
m_defaultManifoldResult(0,0,0),
m_count(0)
{
int i;
for (i=0;i<MAX_BROADPHASE_COLLISION_TYPES;i++)
{
for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
{
m_doubleDispatch[i][j] = 0;
}
}
};
PersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1)
{
gNumManifold2++;
//ASSERT(gNumManifold < 65535);
CollisionObject* body0 = (CollisionObject*)b0;
CollisionObject* body1 = (CollisionObject*)b1;
PersistentManifold* manifold = new PersistentManifold (body0,body1);
m_manifoldsPtr.push_back(manifold);
return manifold;
}
void ParallelIslandDispatcher::ClearManifold(PersistentManifold* manifold)
{
manifold->ClearManifold();
}
void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
{
gNumManifold2--;
//printf("ReleaseManifold: gNumManifold2 %d\n",gNumManifold2);
ClearManifold(manifold);
std::vector<PersistentManifold*>::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 ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
{
int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++)
{
std::vector<PersistentManifold*> islandmanifold;
//int numSleeping = 0;
bool allSleeping = true;
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if (colObj0->GetActivationState()== ACTIVE_TAG)
{
allSleeping = false;
}
if (colObj0->GetActivationState()== DISABLE_DEACTIVATION)
{
allSleeping = false;
}
}
}
for (i=0;i<GetNumManifolds();i++)
{
PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
{
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
{
if (NeedsResponse(*colObj0,*colObj1))
islandmanifold.push_back(manifold);
}
}
}
if (allSleeping)
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
colObj0->SetActivationState( ISLAND_SLEEPING );
}
}
} else
{
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
{
colObj0->SetActivationState( WANTS_DEACTIVATION);
}
}
}
/// Process the actual simulation, only if not sleeping/deactivated
if (islandmanifold.size())
{
callback->ProcessIsland(&islandmanifold[0],islandmanifold.size());
}
}
}
}
CollisionAlgorithm* ParallelIslandDispatcher::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);
}
if (body0->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
} else
{
if (body1->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
}
}
//failed to find an algorithm
return new EmptyAlgorithm(ci);
}
bool ParallelIslandDispatcher::NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1)
{
//here you can do filtering
bool hasResponse =
(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
hasResponse = hasResponse &&
(colObj0.IsActive() || colObj1.IsActive());
return hasResponse;
}
bool ParallelIslandDispatcher::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->IsActive()) && (!body1->IsActive()))
needsCollision = false;
return needsCollision ;
}
///allows the user to get contact point callbacks
ManifoldResult* ParallelIslandDispatcher::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 ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*)
{
}
void ParallelIslandDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)
{
//m_blockedForChanges = true;
int i;
int dispatcherId = GetUniqueId();
for (i=0;i<numPairs;i++)
{
BroadphasePair& pair = pairs[i];
if (dispatcherId>= 0)
{
//dispatcher will keep algorithms persistent in the collision pair
if (!pair.m_algorithms[dispatcherId])
{
pair.m_algorithms[dispatcherId] = 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 = 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;
}

128
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h vendored Normal file
View File

@@ -0,0 +1,128 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef PARALLEL_ISLAND_DISPATCHER_H
#define PARALLEL_ISLAND_DISPATCHER_H
#include "BroadphaseCollision/Dispatcher.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionDispatch/UnionFind.h"
#include "CollisionDispatch/ManifoldResult.h"
#include "BroadphaseCollision/BroadphaseProxy.h"
#include <vector>
class IDebugDraw;
#include "CollisionDispatch/CollisionCreateFunc.h"
///ParallelIslandDispatcher dispatches entire simulation islands in parallel.
///For both collision detection and constraint solving.
///This development heads toward multi-core, CELL SPU and GPU approach
class ParallelIslandDispatcher : public Dispatcher
{
std::vector<PersistentManifold*> 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;
ParallelIslandDispatcher ();
virtual ~ParallelIslandDispatcher() {};
virtual PersistentManifold* GetNewManifold(void* b0,void* b1);
virtual void ReleaseManifold(PersistentManifold* manifold);
virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, 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;}
virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo);
};
#endif //PARALLEL_ISLAND_DISPATCHER_H

194
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp vendored Normal file
View File

@@ -0,0 +1,194 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "ParallelPhysicsEnvironment.h"
#include "CcdPhysicsController.h"
#include "ParallelIslandDispatcher.h"
#include "CollisionDispatch/CollisionWorld.h"
#include "ConstraintSolver/TypedConstraint.h"
#include "CollisionDispatch/SimulationIslandManager.h"
#include "SimulationIsland.h"
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache):
CcdPhysicsEnvironment(dispatcher,pairCache)
{
}
ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
{
}
/// Perform an integration step of duration 'timeStep'.
bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
// Make sure the broadphase / overlapping AABB paircache is up-to-date
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
scene->RefreshOverlappingPairs();
// Find the connected sets that can be simulated in parallel
// Using union find
#ifdef USE_QUICKPROF
Profiler::beginBlock("IslandUnionFind");
#endif //USE_QUICKPROF
GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
{
int i;
int numConstraints = m_constraints.size();
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = m_constraints[i];
const RigidBody* colObj0 = &constraint->GetRigidBodyA();
const RigidBody* colObj1 = &constraint->GetRigidBodyB();
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
{
if (colObj0->IsActive() || colObj1->IsActive())
{
GetSimulationIslandManager()->GetUnionFind().unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
//Store the island id in each body
GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
#ifdef USE_QUICKPROF
Profiler::endBlock("IslandUnionFind");
#endif //USE_QUICKPROF
///build simulation islands
#ifdef USE_QUICKPROF
Profiler::beginBlock("BuildIslands");
#endif //USE_QUICKPROF
std::vector<SimulationIsland> simulationIslands;
simulationIslands.resize(GetNumControllers());
int k;
for (k=0;k<GetNumControllers();k++)
{
CcdPhysicsController* ctrl = m_controllers[k];
int tag = ctrl->GetRigidBody()->m_islandTag1;
if (tag>=0)
{
simulationIslands[tag].m_controllers.push_back(ctrl);
}
}
Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher();
//this is a brute force approach, will rethink later about more subtle ways
int i;
for (i=0;i< scene->GetNumOverlappingPairs();i++)
{
BroadphasePair* pair = &scene->GetOverlappingPair(i);
CollisionObject* col0 = static_cast<CollisionObject*>(pair->m_pProxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject);
if (col0->m_islandTag1 > col1->m_islandTag1)
{
simulationIslands[col0->m_islandTag1].m_overlappingPairIndices.push_back(i);
} else
{
simulationIslands[col1->m_islandTag1].m_overlappingPairIndices.push_back(i);
}
}
//store constraint indices for each island
for (i=0;i<m_constraints.size();i++)
{
TypedConstraint& constraint = *m_constraints[i];
if (constraint.GetRigidBodyA().m_islandTag1 > constraint.GetRigidBodyB().m_islandTag1)
{
simulationIslands[constraint.GetRigidBodyA().m_islandTag1].m_constraintIndices.push_back(i);
} else
{
simulationIslands[constraint.GetRigidBodyB().m_islandTag1].m_constraintIndices.push_back(i);
}
}
//add all overlapping pairs for each island
for (i=0;i<dispatcher->GetNumManifolds();i++)
{
PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
{
int islandTag = colObj0->m_islandTag1;
if (colObj1->m_islandTag1 > islandTag)
islandTag = colObj1->m_islandTag1;
if (dispatcher->NeedsResponse(*colObj0,*colObj1))
simulationIslands[islandTag].m_manifolds.push_back(manifold);
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildIslands");
#endif //USE_QUICKPROF
#ifdef USE_QUICKPROF
Profiler::beginBlock("SimulateIsland");
#endif //USE_QUICKPROF
TypedConstraint** constraintBase = 0;
if (m_constraints.size())
constraintBase = &m_constraints[0];
//Each simulation island can be processed in parallel (will be put on a job queue)
for (k=0;k<simulationIslands.size();k++)
{
if (simulationIslands[k].m_controllers.size())
{
simulationIslands[k].Simulate(m_debugDrawer,m_numIterations, constraintBase ,&scene->GetOverlappingPair(0),dispatcher,GetBroadphase(),m_solver,timeStep);
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SimulateIsland");
#endif //USE_QUICKPROF
return true;
}

44
extern/bullet/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h vendored Normal file
View File

@@ -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 PARALLELPHYSICSENVIRONMENT
#define PARALLELPHYSICSENVIRONMENT
#include "CcdPhysicsEnvironment.h"
class ParallelIslandDispatcher;
/// ParallelPhysicsEnvironment is experimental parallel mainloop for physics simulation
/// Physics Environment takes care of stepping the simulation and is a container for physics entities.
/// It stores rigidbodies,constraints, materials etc.
/// A derived class may be able to 'construct' entities by loading and/or converting
class ParallelPhysicsEnvironment : public CcdPhysicsEnvironment
{
public:
ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
virtual ~ParallelPhysicsEnvironment();
/// Perform an integration step of duration 'timeStep'.
virtual bool proceedDeltaTimeOneStep(float timeStep);
//void BuildSimulationIslands();
};
#endif //PARALLELPHYSICSENVIRONMENT

468
extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp vendored Normal file
View File

@@ -0,0 +1,468 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "SimulationIsland.h"
#include "SimdTransform.h"
#include "CcdPhysicsController.h"
#include "BroadphaseCollision/OverlappingPairCache.h"
#include "CollisionShapes/CollisionShape.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "ConstraintSolver/ContactSolverInfo.h"
#include "ConstraintSolver/ConstraintSolver.h"
#include "ConstraintSolver/TypedConstraint.h"
#include "IDebugDraw.h"
extern float gContactBreakingTreshold;
bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,TypedConstraint** constraintsBaseAddress,BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver* solver,float timeStep)
{
#ifdef USE_QUICKPROF
Profiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
{
// std::vector<CcdPhysicsController*>::iterator i;
int k;
for (k=0;k<GetNumControllers();k++)
{
CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
//todo: only do this when necessary, it's used for contact points
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive())
{
if (!body->IsStatic())
{
body->applyForces( timeStep);
body->integrateVelocities( timeStep);
body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
}
}
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
//BroadphaseInterface* scene = GetBroadphase();
//
// collision detection (?)
//
#ifdef USE_QUICKPROF
Profiler::beginBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
// int numsubstep = m_numIterations;
DispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = debugDrawer;
std::vector<BroadphasePair> overlappingPairs;
overlappingPairs.resize(this->m_overlappingPairIndices.size());
//gather overlapping pair info
int i;
for (i=0;i<m_overlappingPairIndices.size();i++)
{
overlappingPairs[i] = overlappingPairBaseAddress[m_overlappingPairIndices[i]];
}
//pairCache->RefreshOverlappingPairs();
if (overlappingPairs.size())
{
dispatcher->DispatchAllCollisionPairs(&overlappingPairs[0],overlappingPairs.size(),dispatchInfo);///numsubstep,g);
}
//scatter overlapping pair info, mainly the created algorithms/contact caches
for (i=0;i<m_overlappingPairIndices.size();i++)
{
overlappingPairBaseAddress[m_overlappingPairIndices[i]] = overlappingPairs[i];
}
#ifdef USE_QUICKPROF
Profiler::endBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
int numRigidBodies = m_controllers.size();
//contacts
#ifdef USE_QUICKPROF
Profiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF
//solve the regular constraints (point 2 point, hinge, etc)
for (int g=0;g<numSolverIterations;g++)
{
//
// constraint solving
//
int i;
int numConstraints = m_constraintIndices.size();
//point to point constraints
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]];
constraint->BuildJacobian();
constraint->SolveConstraint( timeStep );
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF
/*
//solve the vehicles
#ifdef NEW_BULLET_VEHICLE_SUPPORT
//vehicles
int numVehicles = m_wrapperVehicles.size();
for (int i=0;i<numVehicles;i++)
{
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
vehicle->UpdateVehicle( timeStep);
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
*/
/*
Profiler::beginBlock("CallbackTriggers");
#endif //USE_QUICKPROF
CallbackTriggers();
#ifdef USE_QUICKPROF
Profiler::endBlock("CallbackTriggers");
}
*/
//OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache();
ContactSolverInfo solverInfo;
solverInfo.m_friction = 0.9f;
solverInfo.m_numIterations = numSolverIterations;
solverInfo.m_timeStep = timeStep;
solverInfo.m_restitution = 0.f;//m_restitution;
if (m_manifolds.size())
{
solver->SolveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0);
}
#ifdef USE_QUICKPROF
Profiler::beginBlock("proceedToTransform");
#endif //USE_QUICKPROF
{
{
UpdateAabbs(debugDrawer,broadphase,timeStep);
float toi = 1.f;
//experimental continuous collision detection
/* if (m_ccdMode == 3)
{
DispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
// GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
toi = dispatchInfo.m_timeOfImpact;
}
*/
//
// integrating solution
//
{
std::vector<CcdPhysicsController*>::iterator i;
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = *i;
SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
if (body->IsActive())
{
if (!body->IsStatic())
{
body->predictIntegratedTransform(timeStep* toi, predictedTrans);
body->proceedToTransform( predictedTrans);
}
}
}
}
//
// disable sleeping physics objects
//
std::vector<CcdPhysicsController*> m_sleepingControllers;
std::vector<CcdPhysicsController*>::iterator i;
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep);
if (ctrl->wantsSleeping())
{
if (body->GetActivationState() == ACTIVE_TAG)
body->SetActivationState( WANTS_DEACTIVATION );
} else
{
if (body->GetActivationState() != DISABLE_DEACTIVATION)
body->SetActivationState( ACTIVE_TAG );
}
if (true)
{
if (body->GetActivationState() == ISLAND_SLEEPING)
{
m_sleepingControllers.push_back(ctrl);
}
} else
{
if (ctrl->wantsSleeping())
{
m_sleepingControllers.push_back(ctrl);
}
}
}
}
#ifdef USE_QUICKPROF
Profiler::endBlock("proceedToTransform");
Profiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF
SyncMotionStates(timeStep);
#ifdef USE_QUICKPROF
Profiler::endBlock("SyncMotionStates");
#endif //USE_QUICKPROF
#ifdef NEW_BULLET_VEHICLE_SUPPORT
//sync wheels for vehicles
int numVehicles = m_wrapperVehicles.size();
for (int i=0;i<numVehicles;i++)
{
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
wrapperVehicle->SyncWheels();
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
return true;
}
}
void SimulationIsland::SyncMotionStates(float timeStep)
{
std::vector<CcdPhysicsController*>::iterator i;
//
// synchronize the physics and graphics transformations
//
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
ctrl->SynchronizeMotionStates(timeStep);
}
}
void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* scene,float timeStep)
{
std::vector<CcdPhysicsController*>::iterator i;
//
// update aabbs, only for moving objects (!)
//
for (i=m_controllers.begin();
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
SimdPoint3 minAabb,maxAabb;
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
body->getLinearVelocity(),
//body->getAngularVelocity(),
SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
timeStep,minAabb,maxAabb);
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
minAabb -= manifoldExtraExtents;
maxAabb += manifoldExtraExtents;
BroadphaseProxy* bp = body->m_broadphaseHandle;
if (bp)
{
SimdVector3 color (1,1,0);
class IDebugDraw* m_debugDrawer = 0;
/*
if (m_debugDrawer)
{
//draw aabb
switch (body->GetActivationState())
{
case ISLAND_SLEEPING:
{
color.setValue(1,1,1);
break;
}
case WANTS_DEACTIVATION:
{
color.setValue(0,0,1);
break;
}
case ACTIVE_TAG:
{
break;
}
case DISABLE_DEACTIVATION:
{
color.setValue(1,0,1);
};
};
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}
}
*/
if ( (maxAabb-minAabb).length2() < 1e12f)
{
scene->SetAabb(bp,minAabb,maxAabb);
} else
{
//something went wrong, investigate
//removeCcdPhysicsController(ctrl);
body->SetActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe)
{
reportMe = false;
printf("Overflow in AABB, object removed from simulation \n");
printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
printf("Please include above information, your Platform, version of OS.\n");
printf("Thanks.\n");
}
}
}
}
}

53
extern/bullet/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h vendored Normal file
View File

@@ -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 SIMULATION_ISLAND_H
#define SIMULATION_ISLAND_H
#include <vector>
class BroadphaseInterface;
class Dispatcher;
class IDebugDraw;
///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's
///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's
///At the start of the simulation timestep the simulation islands are re-calculated
///During one timestep there is no merging or splitting of Simulation Islands
class SimulationIsland
{
public:
std::vector<class CcdPhysicsController*> m_controllers;
std::vector<class PersistentManifold*> m_manifolds;
std::vector<int> m_overlappingPairIndices;
std::vector<int> m_constraintIndices;
bool Simulate(IDebugDraw* debugDrawer,int numSolverIterations,class TypedConstraint** constraintsBaseAddress,struct BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase, class ConstraintSolver* solver, float timeStep);
int GetNumControllers()
{
return m_controllers.size();
}
void SyncMotionStates(float timeStep);
void UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* broadphase,float timeStep);
};
#endif //SIMULATION_ISLAND_H

2
extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h vendored
View File

@@ -82,7 +82,7 @@ typedef enum PHY_ConstraintType {
PHY_LINEHINGE_CONSTRAINT=2,
PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket
PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle
PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked'
} PHY_ConstraintType;

2
extern/bullet/Extras/PhysicsInterface/Common/PHY_IPhysicsEnvironment.h vendored
View File

@@ -39,6 +39,8 @@ class PHY_IPhysicsEnvironment
float axisX,float axisY,float axisZ)=0;
virtual void removeConstraint(int constraintid)=0;
virtual float getAppliedImpulse(int constraintid){ return 0.f;}
virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ,
float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ)=0;

42
extern/bullet/LinearMath/SimdScalar.h vendored
View File

@@ -27,29 +27,29 @@ subject to the following restrictions:
#include <float.h>
#ifdef WIN32
#pragma warning(disable:4530)
#pragma warning(disable:4996)
#ifdef __MINGW32__
#define SIMD_FORCE_INLINE inline
#if defined(__MINGW32__) || defined(__CYGWIN__)
#define SIMD_FORCE_INLINE inline
#else
#pragma warning(disable:4530)
#pragma warning(disable:4996)
#define SIMD_FORCE_INLINE __forceinline
#endif //__MINGW32__
//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define ATTRIBUTE_ALIGNED16(a) a
#include <assert.h>
#define ASSERT assert
#else
#define SIMD_FORCE_INLINE __forceinline
#endif //__MINGW32__
//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define ATTRIBUTE_ALIGNED16(a) a
#include <assert.h>
#define ASSERT assert
#else
#define SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a
#ifndef assert
#include <assert.h>
#endif
#define ASSERT assert
//non-windows systems
#define SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a
#ifndef assert
#include <assert.h>
#endif
#define ASSERT assert
#endif

7
extern/bullet/LinearMath/SimdTransformUtil.h vendored
View File

@@ -25,6 +25,13 @@ subject to the following restrictions:
#define SimdRecipSqrt(x) ((float)(1.0f/SimdSqrt(float(x)))) /* reciprocal square root */
inline SimdVector3 SimdAabbSupport(const SimdVector3& halfExtents,const SimdVector3& supportDir)
{
return SimdVector3(supportDir.x() < SimdScalar(0.0f) ? -halfExtents.x() : halfExtents.x(),
supportDir.y() < SimdScalar(0.0f) ? -halfExtents.y() : halfExtents.y(),
supportDir.z() < SimdScalar(0.0f) ? -halfExtents.z() : halfExtents.z());
}
inline void SimdPlaneSpace1 (const SimdVector3& n, SimdVector3& p, SimdVector3& q)
{

2
extern/bullet/LinearMath/SimdVector3.h vendored
View File

@@ -183,7 +183,7 @@ operator+(const SimdVector3& v1, const SimdVector3& v2)
SIMD_FORCE_INLINE SimdVector3
operator*(const SimdVector3& v1, const SimdVector3& v2)
{
return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() *+ v2.z());
return SimdVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
}
SIMD_FORCE_INLINE SimdVector3

2
source/gameengine/Ketsji/KX_ConvertPhysicsObjects.cpp
View File

@@ -1091,7 +1091,7 @@ void KX_ConvertBulletObject( class KX_GameObject* gameobj,
if (objprop->m_disableSleeping)
physicscontroller->GetRigidBody()->SetActivationState(DISABLE_DEACTIVATION);
if (!objprop->m_angular_rigidbody)
if (objprop->m_dyna && !objprop->m_angular_rigidbody)
{
/*
//setting the inertia could achieve similar results to constraint the up

31
source/gameengine/Physics/Bullet/CcdPhysicsController.cpp
View File

@@ -34,7 +34,7 @@ class BP_Proxy;
float gDeactivationTime = 2.f;
bool gDisableDeactivation = false;
float gLinearSleepingTreshold = 0.4f;
float gLinearSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.0f;
#include "Dynamics/MassProps.h"
@@ -210,28 +210,6 @@ void CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionsta
}
void CcdPhysicsController::SetMargin(float margin)
{
if (m_body && m_body->GetCollisionShape())
{
m_body->GetCollisionShape()->SetMargin(margin);
}
}
float CcdPhysicsController::GetMargin() const
{
if (m_body && m_body->GetCollisionShape())
{
return m_body->GetCollisionShape()->GetMargin();
}
return 0.f;
}
// kinematic methods
void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
{
@@ -557,6 +535,7 @@ PHY_IPhysicsController* CcdPhysicsController::GetReplica()
DefaultMotionState::DefaultMotionState()
{
m_worldTransform.setIdentity();
m_localScaling.setValue(1.f,1.f,1.f);
}
@@ -574,9 +553,9 @@ void DefaultMotionState::getWorldPosition(float& posX,float& posY,float& posZ)
void DefaultMotionState::getWorldScaling(float& scaleX,float& scaleY,float& scaleZ)
{
scaleX = 1.;
scaleY = 1.;
scaleZ = 1.;
scaleX = m_localScaling.getX();
scaleY = m_localScaling.getY();
scaleZ = m_localScaling.getZ();
}
void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,float& quatIma2,float& quatReal)

49
source/gameengine/Physics/Bullet/CcdPhysicsController.h
View File

@@ -39,20 +39,38 @@ extern bool gDisableDeactivation;
class CcdPhysicsEnvironment;
struct CcdConstructionInfo
{
///CollisionFilterGroups provides some optional usage of basic collision filtering
///this is done during broadphase, so very early in the pipeline
///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision
enum CollisionFilterGroups
{
DefaultFilter = 1,
StaticFilter = 2,
KinematicFilter = 4,
DebrisFilter = 8,
AllFilter = DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter,
};
CcdConstructionInfo()
: m_gravity(0,0,0),
m_scaling(1.f,1.f,1.f),
m_mass(0.f),
m_restitution(0.1f),
m_friction(0.5f),
m_linearDamping(0.1f),
m_angularDamping(0.1f),
m_collisionFlags(0),
m_collisionFilterGroup(DefaultFilter),
m_collisionFilterMask(AllFilter),
m_MotionState(0),
m_physicsEnv(0),
m_inertiaFactor(1.f),
m_scaling(1.f,1.f,1.f),
m_collisionFlags(0)
m_inertiaFactor(1.f)
{
}
@@ -66,6 +84,15 @@ struct CcdConstructionInfo
SimdScalar m_angularDamping;
int m_collisionFlags;
///optional use of collision group/mask:
///only collision with object goups that match the collision mask.
///this is very basic early out. advanced collision filtering should be
///done in the CollisionDispatcher::NeedsCollision and NeedsResponse
///both values default to 1
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
CollisionShape* m_collisionShape;
class PHY_IMotionState* m_MotionState;
@@ -158,10 +185,21 @@ class CcdPhysicsController : public PHY_IPhysicsController
virtual void setNewClientInfo(void* clientinfo);
virtual PHY_IPhysicsController* GetReplica();
///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
short int GetCollisionFilterGroup() const
{
return m_cci.m_collisionFilterGroup;
}
///There should be no 'SetCollisionFilterGroup' method, as changing this during run-time is will result in errors
short int GetCollisionFilterMask() const
{
return m_cci.m_collisionFilterMask;
}
virtual void calcXform() {} ;
virtual void SetMargin(float margin);
virtual float GetMargin() const;
virtual void SetMargin(float margin) {};
virtual float GetMargin() const {return 0.f;};
bool wantsSleeping();
@@ -208,6 +246,7 @@ class DefaultMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations();
SimdTransform m_worldTransform;
SimdVector3 m_localScaling;
};

254
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp
View File

@@ -30,14 +30,12 @@ subject to the following restrictions:
#include "CollisionShapes/ConvexShape.h"
#include "CollisionShapes/ConeShape.h"
#include "CollisionDispatch/SimulationIslandManager.h"
#include "BroadphaseCollision/Dispatcher.h"
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "CollisionShapes/TriangleMeshShape.h"
#include "ConstraintSolver/OdeConstraintSolver.h"
#include "ConstraintSolver/SimpleConstraintSolver.h"
#include "ConstraintSolver/SequentialImpulseConstraintSolver.h"
//profiling/timings
@@ -56,7 +54,7 @@ subject to the following restrictions:
#include "PHY_IMotionState.h"
#include "CollisionDispatch/EmptyCollisionAlgorithm.h"
#include "CollisionDispatch/UnionFind.h"
#include "CollisionShapes/SphereShape.h"
@@ -77,6 +75,7 @@ RaycastVehicle::VehicleTuning gTuning;
#include "ConstraintSolver/ConstraintSolver.h"
#include "ConstraintSolver/Point2PointConstraint.h"
#include "ConstraintSolver/HingeConstraint.h"
#include "ConstraintSolver/Generic6DofConstraint.h"
//#include "BroadphaseCollision/QueryDispatcher.h"
@@ -322,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,BroadphaseInterface* broadphase)
CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache)
:m_scalingPropagated(false),
m_numIterations(10),
m_numTimeSubSteps(1),
@@ -336,18 +335,19 @@ m_enableSatCollisionDetection(false)
{
m_triggerCallbacks[i] = 0;
}
if (!dispatcher)
dispatcher = new CollisionDispatcher();
if(!broadphase)
if(!pairCache)
{
//todo: calculate/let user specify this world sizes
SimdVector3 worldMin(-10000,-10000,-10000);
SimdVector3 worldMax(10000,10000,10000);
broadphase = new AxisSweep3(worldMin,worldMax);
pairCache = new AxisSweep3(worldMin,worldMax);
//broadphase = new SimpleBroadphase();
}
@@ -355,11 +355,13 @@ m_enableSatCollisionDetection(false)
setSolverType(1);//issues with quickstep and memory allocations
m_collisionWorld = new CollisionWorld(dispatcher,broadphase);
m_collisionWorld = new CollisionWorld(dispatcher,pairCache);
m_debugDrawer = 0;
m_gravity = SimdVector3(0.f,-10.f,0.f);
m_islandManager = new SimulationIslandManager();
}
@@ -373,7 +375,7 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
body->setGravity( m_gravity );
m_controllers.push_back(ctrl);
m_collisionWorld->AddCollisionObject(body);
m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask());
assert(body->m_broadphaseHandle);
@@ -385,7 +387,8 @@ void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl)
assert(shapeinterface);
const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
SimdPoint3 minAabb,maxAabb;
@@ -637,6 +640,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
if (body->IsActive())
{
if (!body->IsStatic())
@@ -654,7 +660,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
Profiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
BroadphaseInterface* scene = GetBroadphase();
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
//
@@ -674,8 +680,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = this->m_debugDrawer;
scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g);
scene->RefreshOverlappingPairs();
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
#ifdef USE_QUICKPROF
@@ -685,7 +693,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
int numRigidBodies = m_controllers.size();
m_collisionWorld->UpdateActivationState();
m_islandManager->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
{
int i;
@@ -702,15 +711,15 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
if (colObj0->IsActive() || colObj1->IsActive())
{
GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1,
m_islandManager->GetUnionFind().unite((colObj0)->m_islandTag1,
(colObj1)->m_islandTag1);
}
}
}
}
m_collisionWorld->StoreIslandActivationState();
m_islandManager->StoreIslandActivationState(GetCollisionWorld());
//contacts
@@ -762,7 +771,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#endif //NEW_BULLET_VEHICLE_SUPPORT
struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback
struct InplaceSolverIslandCallback : public SimulationIslandManager::IslandCallback
{
ContactSolverInfo& m_solverInfo;
@@ -803,7 +812,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#endif //USE_QUICKPROF
/// solve all the contact points and contact friction
GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
m_islandManager->BuildAndProcessIslands(GetCollisionWorld()->GetDispatcher(),m_collisionWorld->GetCollisionObjectArray(),&solverCallback);
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildAndProcessIslands");
@@ -842,7 +851,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g);
//pairCache->RefreshOverlappingPairs();//??
GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo);
toi = dispatchInfo.m_timeOfImpact;
}
@@ -871,6 +882,11 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
if (!body->IsStatic())
{
if (body->m_hitFraction < 1.f)
{
//set velocity to zero... until we have proper CCD integrated
body->setLinearVelocity(body->getLinearVelocity()*0.5f);
}
body->predictIntegratedTransform(timeStep* toi, predictedTrans);
body->proceedToTransform( predictedTrans);
}
@@ -1033,7 +1049,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType)
if (m_solverType != solverType)
{
m_solver = new SimpleConstraintSolver();
m_solver = new SequentialImpulseConstraintSolver();
break;
}
@@ -1043,7 +1059,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType)
default:
if (m_solverType != solverType)
{
m_solver = new OdeConstraintSolver();
// m_solver = new OdeConstraintSolver();
break;
}
@@ -1182,8 +1198,8 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA;
SimdVector3 axisInA(axisX,axisY,axisZ);
SimdVector3 axisInB = rb1 ?
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * -axisInA;
(rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * axisInA)) :
rb0->getCenterOfMassTransform().getBasis() * axisInA;
bool angularOnly = false;
@@ -1213,9 +1229,55 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
break;
}
case PHY_GENERIC_6DOF_CONSTRAINT:
{
Generic6DofConstraint* genericConstraint = 0;
if (rb1)
{
SimdTransform frameInA;
SimdTransform frameInB;
SimdVector3 axis1, axis2;
SimdPlaneSpace1( axisInA, axis1, axis2 );
frameInA.getBasis().setValue( axisInA.x(), axis1.x(), axis2.x(),
axisInA.y(), axis1.y(), axis2.y(),
axisInA.z(), axis1.z(), axis2.z() );
SimdPlaneSpace1( axisInB, axis1, axis2 );
frameInB.getBasis().setValue( axisInB.x(), axis1.x(), axis2.x(),
axisInB.y(), axis1.y(), axis2.y(),
axisInB.z(), axis1.z(), axis2.z() );
frameInA.setOrigin( pivotInA );
frameInB.setOrigin( pivotInB );
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
} else
{
// TODO: Implement single body case...
}
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(type);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
break;
}
case PHY_ANGULAR_CONSTRAINT:
angularOnly = true;
case PHY_LINEHINGE_CONSTRAINT:
{
HingeConstraint* hinge = 0;
@@ -1271,21 +1333,62 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
//Following the COLLADA physics specification for constraints
int CcdPhysicsEnvironment::createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& frameInA,
SimdTransform& frameInB,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
)
{
//we could either add some logic to recognize ball-socket and hinge, or let that up to the user
//perhaps some warning or hint that hinge/ball-socket is more efficient?
Generic6DofConstraint* genericConstraint = 0;
CcdPhysicsController* ctrl0 = (CcdPhysicsController*) ctrlRef;
CcdPhysicsController* ctrl1 = (CcdPhysicsController*) ctrlOther;
RigidBody* rb0 = ctrl0->GetRigidBody();
RigidBody* rb1 = ctrl1->GetRigidBody();
if (rb1)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
genericConstraint = new Generic6DofConstraint(
*rb0,*rb1,
frameInA,frameInB);
genericConstraint->setLinearLowerLimit(linearMinLimits);
genericConstraint->setLinearUpperLimit(linearMaxLimits);
genericConstraint->setAngularLowerLimit(angularMinLimits);
genericConstraint->setAngularUpperLimit(angularMaxLimits);
} else
{
// TODO: Implement single body case...
//No, we can use a fixed rigidbody in above code, rather then unnecessary duplation of code
}
return 0.f;
if (genericConstraint)
{
m_constraints.push_back(genericConstraint);
genericConstraint->SetUserConstraintId(gConstraintUid++);
genericConstraint->SetUserConstraintType(PHY_GENERIC_6DOF_CONSTRAINT);
//64 bit systems can't cast pointer to int. could use size_t instead.
return genericConstraint->GetUserConstraintId();
}
return 0;
}
void CcdPhysicsEnvironment::removeConstraint(int constraintId)
{
std::vector<TypedConstraint*>::iterator i;
@@ -1296,15 +1399,7 @@ void CcdPhysicsEnvironment::removeConstraint(int constraintId)
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintId)
{
//activate objects
if (constraint->GetRigidBodyA().mergesSimulationIslands())
constraint->GetRigidBodyA().activate();
if (constraint->GetRigidBodyB().mergesSimulationIslands())
constraint->GetRigidBodyB().activate();
std::swap(*i, m_constraints.back());
m_constraints.pop_back();
break;
}
@@ -1352,13 +1447,6 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
SimdVector3 rayFrom(fromX,fromY,fromZ);
SimdVector3 rayTo(toX,toY,toZ);
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 color (1,0,0);
m_debugDrawer->DrawLine(rayFrom,rayTo,color);
}
SimdVector3 hitPointWorld,normalWorld;
//Either Ray Cast with or without filtering
@@ -1376,26 +1464,10 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::rayTest(PHY_IPhysicsController* i
hitX = rayCallback.m_hitPointWorld.getX();
hitY = rayCallback.m_hitPointWorld.getY();
hitZ = rayCallback.m_hitPointWorld.getZ();
if (rayCallback.m_hitNormalWorld.length2() > SIMD_EPSILON)
{
rayCallback.m_hitNormalWorld.normalize();
}
normalX = rayCallback.m_hitNormalWorld.getX();
normalY = rayCallback.m_hitNormalWorld.getY();
normalZ = rayCallback.m_hitNormalWorld.getZ();
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
{
SimdVector3 colorNormal(0,0,1);
m_debugDrawer->DrawLine(rayCallback.m_hitPointWorld,rayCallback.m_hitPointWorld+rayCallback.m_hitNormalWorld,colorNormal);
SimdVector3 color (0,1,0);
m_debugDrawer->DrawLine(rayFrom,rayCallback.m_hitPointWorld,color);
}
}
@@ -1425,15 +1497,6 @@ BroadphaseInterface* CcdPhysicsEnvironment::GetBroadphase()
const CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher() const
{
return m_collisionWorld->GetDispatcher();
}
CollisionDispatcher* CcdPhysicsEnvironment::GetDispatcher()
{
return m_collisionWorld->GetDispatcher();
}
CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
{
@@ -1448,6 +1511,8 @@ CcdPhysicsEnvironment::~CcdPhysicsEnvironment()
//first delete scene, then dispatcher, because pairs have to release manifolds on the dispatcher
//delete m_dispatcher;
delete m_collisionWorld;
delete m_islandManager;
}
@@ -1464,15 +1529,9 @@ CcdPhysicsController* CcdPhysicsEnvironment::GetPhysicsController( int index)
}
int CcdPhysicsEnvironment::GetNumManifolds() const
{
return GetDispatcher()->GetNumManifolds();
}
const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const
{
return GetDispatcher()->GetManifoldByIndexInternal(index);
}
TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId)
{
@@ -1565,9 +1624,10 @@ void CcdPhysicsEnvironment::requestCollisionCallback(PHY_IPhysicsController* ctr
void CcdPhysicsEnvironment::CallbackTriggers()
{
CcdPhysicsController* ctrl0=0,*ctrl1=0;
if (m_triggerCallbacks[PHY_OBJECT_RESPONSE])
if (m_triggerCallbacks[PHY_OBJECT_RESPONSE] || (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints)))
{
//walk over all overlapping pairs, and if one of the involved bodies is registered for trigger callback, perform callback
int numManifolds = m_collisionWorld->GetDispatcher()->GetNumManifolds();
@@ -1577,6 +1637,16 @@ void CcdPhysicsEnvironment::CallbackTriggers()
int numContacts = manifold->GetNumContacts();
if (numContacts)
{
if (m_debugDrawer && (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawContactPoints))
{
for (int j=0;j<numContacts;j++)
{
SimdVector3 color(1,0,0);
const ManifoldPoint& cp = manifold->GetContactPoint(j);
if (m_debugDrawer)
m_debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
}
}
RigidBody* obj0 = static_cast<RigidBody* >(manifold->GetBody0());
RigidBody* obj1 = static_cast<RigidBody* >(manifold->GetBody1());
@@ -1603,6 +1673,7 @@ void CcdPhysicsEnvironment::CallbackTriggers()
}
}
@@ -1764,3 +1835,18 @@ PHY_IPhysicsController* CcdPhysicsEnvironment::CreateConeController(float conera
return sphereController;
}
float CcdPhysicsEnvironment::getAppliedImpulse(int constraintid)
{
std::vector<TypedConstraint*>::iterator i;
for (i=m_constraints.begin();
!(i==m_constraints.end()); i++)
{
TypedConstraint* constraint = (*i);
if (constraint->GetUserConstraintId() == constraintid)
{
return constraint->GetAppliedImpulse();
}
}
return 0.f;
}

70
source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h
View File

@@ -20,11 +20,13 @@ subject to the following restrictions:
#include <vector>
class CcdPhysicsController;
#include "SimdVector3.h"
#include "SimdTransform.h"
class TypedConstraint;
class SimulationIslandManager;
class CollisionDispatcher;
class Dispatcher;
//#include "BroadphaseInterface.h"
@@ -37,6 +39,7 @@ class Dispatcher;
class WrapperVehicle;
class PersistentManifold;
class BroadphaseInterface;
class OverlappingPairCache;
class IDebugDraw;
/// CcdPhysicsEnvironment is experimental mainloop for physics simulation using optional continuous collision detection.
@@ -47,12 +50,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
{
SimdVector3 m_gravity;
protected:
IDebugDraw* m_debugDrawer;
//solver iterations
int m_numIterations;
//timestep subdivisions
int m_numTimeSubSteps;
int m_ccdMode;
int m_solverType;
int m_profileTimings;
@@ -60,9 +68,10 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
ContactSolverInfo m_solverInfo;
SimulationIslandManager* m_islandManager;
public:
CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
virtual ~CcdPhysicsEnvironment();
@@ -98,7 +107,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
virtual void endFrame() {};
/// Perform an integration step of duration 'timeStep'.
virtual bool proceedDeltaTime(double curTime,float timeStep);
bool proceedDeltaTimeOneStep(float timeStep);
virtual bool proceedDeltaTimeOneStep(float timeStep);
virtual void setFixedTimeStep(bool useFixedTimeStep,float fixedTimeStep){};
//returns 0.f if no fixed timestep is used
@@ -112,10 +121,25 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
virtual int createConstraint(class PHY_IPhysicsController* ctrl,class PHY_IPhysicsController* ctrl2,PHY_ConstraintType type,
float pivotX,float pivotY,float pivotZ,
float axisX,float axisY,float axisZ);
virtual void removeConstraint(int constraintid);
virtual float getAppliedImpulse(int constraintid);
//Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
);
virtual void removeConstraint(int constraintid);
virtual float getAppliedImpulse(int constraintid);
virtual void CallbackTriggers();
@@ -160,9 +184,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
BroadphaseInterface* GetBroadphase();
CollisionDispatcher* GetDispatcher();
const CollisionDispatcher* GetDispatcher() const;
bool IsSatCollisionDetectionEnabled() const
{
@@ -180,16 +204,39 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
CcdPhysicsController* GetPhysicsController( int index);
int GetNumManifolds() const;
const PersistentManifold* GetManifold(int index) const;
std::vector<TypedConstraint*> m_constraints;
private:
void SyncMotionStates(float timeStep);
class CollisionWorld* GetCollisionWorld()
{
return m_collisionWorld;
}
const class CollisionWorld* GetCollisionWorld() const
{
return m_collisionWorld;
}
SimulationIslandManager* GetSimulationIslandManager()
{
return m_islandManager;
}
const SimulationIslandManager* GetSimulationIslandManager() const
{
return m_islandManager;
}
protected:
std::vector<CcdPhysicsController*> m_controllers;
@@ -206,6 +253,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
bool m_scalingPropagated;
};

55
source/gameengine/Physics/common/PHY_DynamicTypes.h
View File

@@ -1,38 +1,24 @@
/**
* $Id$
*
* ***** 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 *****
*/
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef __PHY_DYNAMIC_TYPES
#define __PHY_DYNAMIC_TYPES
class PHY_ResponseTable;
class PHY_Shape;
@@ -68,11 +54,6 @@ typedef enum
PHY__Vector3 m_normal; /* point2 - point1 */
} PHY_CollData;
/* A response callback is called by SOLID for each pair of collding objects. 'client-data'
is a pointer to an arbitrary structure in the client application. The client objects are
pointers to structures in the client application associated with the coliding objects.
'coll_data' is the collision data computed by SOLID.
*/
typedef bool (*PHY_ResponseCallback)(void *client_data,
void *client_object1,
@@ -101,9 +82,11 @@ typedef enum PHY_ConstraintType {
PHY_LINEHINGE_CONSTRAINT=2,
PHY_ANGULAR_CONSTRAINT = 3,//hinge without ball socket
PHY_VEHICLE_CONSTRAINT=11,//complex 'constraint' that turns a rigidbody into a vehicle
PHY_GENERIC_6DOF_CONSTRAINT=12,//can leave any of the 6 degree of freedom 'free' or 'locked'
} PHY_ConstraintType;
typedef float PHY_Vector3[3];
#endif //__PHY_DYNAMIC_TYPES

1
source/gameengine/Physics/common/PHY_IPhysicsEnvironment.h
View File

@@ -90,6 +90,7 @@ class PHY_IPhysicsEnvironment
virtual void removeConstraint(int constraintid)=0;
virtual float getAppliedImpulse(int constraintid){ return 0.f;}
//complex constraint for vehicles
virtual PHY_IVehicle* getVehicleConstraint(int constraintId) =0;