fixed several internal Bullet rigidbody dynamics bugs:

- broadphase had bugs in removing objects,
- persistent manifold renamed value,
- cylinder penetration depth fixed,
- memory leak for persistent manifold
This commit is contained in:
Erwin Coumans 2006-04-26 03:20:28 +00:00
parent 94dd1085e3
commit 45d0123a59
24 changed files with 135 additions and 80 deletions

@ -125,9 +125,10 @@ void AxisSweep3::Quantize(unsigned short* out, const SimdPoint3& point, int isMa
clampedPoint.setMin(m_worldAabbMax);
SimdVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
out[0] = (unsigned short)(((int)v.getX() & 0xfffe) | isMax);
out[1] = (unsigned short)(((int)v.getY() & 0xfffe) | isMax);
out[2] = (unsigned short)(((int)v.getZ() & 0xfffe) | isMax);
out[0] = (unsigned short)(((int)v.getX() & 0xfffc) | isMax);
out[1] = (unsigned short)(((int)v.getY() & 0xfffc) | isMax);
out[2] = (unsigned short)(((int)v.getZ() & 0xfffc) | isMax);
}
@ -164,7 +165,11 @@ unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3&
// allocate a handle
unsigned short handle = AllocHandle();
assert(handle!= 0xcdcd);
Handle* pHandle = GetHandle(handle);
pHandle->m_handleId = handle;
//pHandle->m_pOverlaps = 0;
pHandle->m_clientObject = pOwner;
@ -202,6 +207,7 @@ unsigned short AxisSweep3::AddHandle(const SimdPoint3& aabbMin,const SimdPoint3&
return handle;
}
void AxisSweep3::RemoveHandle(unsigned short handle)
{
Handle* pHandle = GetHandle(handle);
@ -211,9 +217,19 @@ void AxisSweep3::RemoveHandle(unsigned short handle)
// compute current limit of edge arrays
int limit = m_numHandles * 2;
int axis;
for (axis = 0;axis<3;axis++)
{
Edge* pEdges = m_pEdges[axis];
int maxEdge= pHandle->m_maxEdges[axis];
pEdges[maxEdge].m_pos = 0xffff;
int minEdge = pHandle->m_minEdges[axis];
pEdges[minEdge].m_pos = 0xffff;
}
// remove the edges by sorting them up to the end of the list
for (int axis = 0; axis < 3; axis++)
for ( axis = 0; axis < 3; axis++)
{
Edge* pEdges = m_pEdges[axis];
int max = pHandle->m_maxEdges[axis];

@ -43,6 +43,7 @@ public:
class Handle : public BroadphaseProxy
{
public:
// indexes into the edge arrays
unsigned short m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12
unsigned short m_handleId;

@ -50,7 +50,7 @@ CONCAVE_SHAPES_START_HERE,
///BroadphaseProxy
struct BroadphaseProxy
{
//Usually the client CollisionObject or Rigidbody class
void* m_clientObject;

@ -23,6 +23,18 @@ subject to the following restrictions:
#include <vector>
void SimpleBroadphase::validate()
{
for (int i=0;i<m_numProxies;i++)
{
for (int j=i+1;j<m_numProxies;j++)
{
assert(m_pProxies[i] != m_pProxies[j]);
}
}
}
SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
:m_firstFreeProxy(0),
m_numProxies(0),
@ -34,7 +46,7 @@ SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap)
m_proxies = new SimpleBroadphaseProxy[maxProxies];
m_freeProxies = new int[maxProxies];
m_pProxies = new BroadphaseProxy*[maxProxies];
m_pProxies = new SimpleBroadphaseProxy*[maxProxies];
m_OverlappingPairs = new BroadphasePair[maxOverlap];
@ -72,11 +84,17 @@ 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];
BroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr);
SimpleBroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(min,max,shapeType,userPtr);
m_firstFreeProxy++;
SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
int index = proxy - proxy1;
assert(index == freeIndex);
m_pProxies[m_numProxies] = proxy;
m_numProxies++;
//validate();
return proxy;
}
@ -95,41 +113,34 @@ void SimpleBroadphase::RemoveOverlappingPairsContainingProxy(BroadphaseProxy* pr
}
}
void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxy)
void SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxyOrg)
{
int i;
BroadphaseProxy* proxy1 = &m_proxies[0];
SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxyOrg);
SimpleBroadphaseProxy* proxy1 = &m_proxies[0];
int index = proxy - proxy1;
int index = proxy0 - proxy1;
assert (index < m_maxProxies);
m_freeProxies[--m_firstFreeProxy] = index;
RemoveOverlappingPairsContainingProxy(proxy);
RemoveOverlappingPairsContainingProxy(proxyOrg);
for (i=0;i<m_numProxies;i++)
{
if (m_pProxies[i] == proxy)
if (m_pProxies[i] == proxyOrg)
{
m_proxies[i] = m_proxies[m_numProxies-1];
m_pProxies[i] = m_pProxies[m_numProxies-1];
break;
}
}
m_numProxies--;
//validate();
}
SimpleBroadphaseProxy* SimpleBroadphase::GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
{
SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
int index = proxy0 - &m_proxies[0];
//assert(index < m_numProxies);
SimpleBroadphaseProxy* sbp = &m_proxies[index];
return sbp;
}
void SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
{
SimpleBroadphaseProxy* sbp = GetSimpleProxyFromProxy(proxy);
@ -167,7 +178,8 @@ void SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
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;
@ -181,11 +193,14 @@ void SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProx
if (m_NumOverlapBroadphasePair >= m_maxOverlap)
{
printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
//printf("Error: too many overlapping objects: m_NumOverlapBroadphasePair: %d\n",m_NumOverlapBroadphasePair);
assert(0);
} else
{
m_NumOverlapBroadphasePair++;
}
m_NumOverlapBroadphasePair++;
}
BroadphasePair* SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)

@ -47,7 +47,7 @@ class SimpleBroadphase : public BroadphaseInterface
int* m_freeProxies;
int m_firstFreeProxy;
BroadphaseProxy** m_pProxies;
SimpleBroadphaseProxy** m_pProxies;
int m_numProxies;
//during the dispatch, check that user doesn't destroy/create proxy
@ -59,10 +59,16 @@ class SimpleBroadphase : public BroadphaseInterface
int m_maxProxies;
int m_maxOverlap;
SimpleBroadphaseProxy* GetSimpleProxyFromProxy(BroadphaseProxy* proxy);
inline SimpleBroadphaseProxy* GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
{
SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
return proxy0;
}
bool AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1);
void validate();
protected:
void RemoveOverlappingPair(BroadphasePair& pair);
void CleanOverlappingPair(BroadphasePair& pair);

@ -97,6 +97,8 @@ void CollisionDispatcher::ReleaseManifold(PersistentManifold* manifold)
{
std::swap(*i, m_manifoldsPtr.back());
m_manifoldsPtr.pop_back();
delete manifold;
}

@ -119,13 +119,6 @@ void ConvexConvexAlgorithm ::SetLowLevelOfDetail(bool useLowLevel)
m_lowLevelOfDetail = useLowLevel;
}
float ConvexConvexAlgorithm::GetCollisionImpulse() const
{
if (m_manifoldPtr)
return m_manifoldPtr->GetCollisionImpulse();
return 0.f;
}
class FlippedContactResult : public DiscreteCollisionDetectorInterface::Result
@ -329,7 +322,7 @@ void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy
{
m_gjkPairDetector.SetMinkowskiA(min0);
m_gjkPairDetector.SetMinkowskiB(min1);
input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetManifoldMargin();
input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
}

@ -55,7 +55,7 @@ public:
void SetLowLevelOfDetail(bool useLowLevel);
float GetCollisionImpulse() const;
const PersistentManifold* GetManifold()
{

@ -27,7 +27,7 @@ ManifoldResult::ManifoldResult(CollisionObject* body0,CollisionObject* body1,Per
void ManifoldResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
if (depth > m_manifoldPtr->GetManifoldMargin())
if (depth > m_manifoldPtr->GetContactBreakingTreshold())
return;
SimdTransform transAInv = m_body0->m_worldTransform.inverse();

@ -68,7 +68,7 @@ void ConvexTriangleCallback::ProcessTriangle(SimdVector3* triangle)
gjkDetector.SetMinkowskiA(&tm);
gjkDetector.SetMinkowskiB(m_convexShape);
input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetManifoldMargin();
input.m_maximumDistanceSquared = tm.GetMargin()+ m_convexShape->GetMargin() + m_manifoldPtr->GetContactBreakingTreshold();
input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
input.m_maximumDistanceSquared = 1e30f;//?

@ -35,7 +35,7 @@ CylinderShapeZ::CylinderShapeZ (const SimdVector3& halfExtents)
SimdVector3 CylinderLocalSupportX(const SimdVector3& halfExtents,const SimdVector3& v)
inline SimdVector3 CylinderLocalSupportX(const SimdVector3& halfExtents,const SimdVector3& v)
{
const int cylinderUpAxis = 0;
const int XX = 1;
@ -78,7 +78,7 @@ const int ZZ = 2;
SimdVector3 CylinderLocalSupportY(const SimdVector3& halfExtents,const SimdVector3& v)
inline SimdVector3 CylinderLocalSupportY(const SimdVector3& halfExtents,const SimdVector3& v)
{
const int cylinderUpAxis = 1;
@ -113,7 +113,7 @@ const int ZZ = 2;
}
SimdVector3 CylinderLocalSupportZ(const SimdVector3& halfExtents,const SimdVector3& v)
inline SimdVector3 CylinderLocalSupportZ(const SimdVector3& halfExtents,const SimdVector3& v)
{
const int cylinderUpAxis = 2;
const int XX = 0;
@ -155,6 +155,8 @@ SimdVector3 CylinderShapeX::LocalGetSupportingVertexWithoutMargin(const SimdVect
{
return CylinderLocalSupportX(GetHalfExtents(),vec);
}
SimdVector3 CylinderShapeZ::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const
{
return CylinderLocalSupportZ(GetHalfExtents(),vec);
@ -164,9 +166,31 @@ SimdVector3 CylinderShape::LocalGetSupportingVertexWithoutMargin(const SimdVecto
return CylinderLocalSupportY(GetHalfExtents(),vec);
}
void CylinderShape::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i] = CylinderLocalSupportY(GetHalfExtents(),vectors[i]);
}
}
void CylinderShapeZ::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i] = CylinderLocalSupportZ(GetHalfExtents(),vectors[i]);
}
}
void CylinderShapeX::BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const
{
for (int i=0;i<numVectors;i++)
{
supportVerticesOut[i] = CylinderLocalSupportX(GetHalfExtents(),vectors[i]);
}
}

@ -36,6 +36,8 @@ public:
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
virtual SimdVector3 LocalGetSupportingVertex(const SimdVector3& vec) const
{
@ -74,6 +76,7 @@ public:
CylinderShapeX (const SimdVector3& halfExtents);
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
};
class CylinderShapeZ : public CylinderShape
@ -82,6 +85,7 @@ public:
CylinderShapeZ (const SimdVector3& halfExtents);
virtual SimdVector3 LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec)const;
virtual void BatchedUnitVectorGetSupportingVertexWithoutMargin(const SimdVector3* vectors,SimdVector3* supportVerticesOut,int numVectors) const;
};

@ -27,7 +27,7 @@ ManifoldContactAddResult::ManifoldContactAddResult(SimdTransform transA,SimdTran
void ManifoldContactAddResult::AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
{
if (depth > m_manifoldPtr->GetManifoldMargin())
if (depth > m_manifoldPtr->GetContactBreakingTreshold())
return;

@ -82,7 +82,7 @@ int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt)
int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
{
SimdScalar shortestDist = GetManifoldMargin() * GetManifoldMargin();
SimdScalar shortestDist = GetContactBreakingTreshold() * GetContactBreakingTreshold();
int size = GetNumContacts();
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
@ -121,7 +121,7 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
ReplaceContactPoint(newPoint,insertIndex);
}
float PersistentManifold::GetManifoldMargin() const
float PersistentManifold::GetContactBreakingTreshold() const
{
return gContactBreakingTreshold;
}
@ -157,7 +157,7 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > GetManifoldMargin()*GetManifoldMargin() )
if (distance2d > GetContactBreakingTreshold()*GetContactBreakingTreshold() )
{
RemoveContactPoint(i);
}
@ -166,32 +166,6 @@ void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const Sim
}
//todo: remove this treshold
float gPenetrationDistanceCheck = -0.05f;
float PersistentManifold::GetCollisionImpulse() const
{
float averageImpulse = 0.f;
if (GetNumContacts() > 0)
{
float totalImpulse = 0.f;
//return the sum of the applied impulses on the box
for (int i=0;i<GetNumContacts();i++)
{
const ManifoldPoint& cp = GetContactPoint(i);
//avoid conflic noice
if ( cp.GetDistance() <gPenetrationDistanceCheck)
return 0.f;
totalImpulse += cp.m_appliedImpulse;
}
averageImpulse = totalImpulse / ((float)GetNumContacts());
}
return averageImpulse;
}

@ -86,7 +86,7 @@ public:
}
/// todo: get this margin from the current physics / collision environment
float GetManifoldMargin() const;
float GetContactBreakingTreshold() const;
int GetCacheEntry(const ManifoldPoint& newPoint) const;
@ -105,14 +105,13 @@ public:
bool ValidContactDistance(const ManifoldPoint& pt) const
{
return pt.m_distance1 <= GetManifoldMargin();
return pt.m_distance1 <= GetContactBreakingTreshold();
}
/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
void RefreshContactPoints( const SimdTransform& trA,const SimdTransform& trB);
void ClearManifold();
float GetCollisionImpulse() const;
};

@ -21,7 +21,7 @@ subject to the following restrictions:
#include "SimdVector3.h"
#include "SimdPoint3.h"
#define NO_VIRTUAL_INTERFACE
#define NO_VIRTUAL_INTERFACE 1
#ifdef NO_VIRTUAL_INTERFACE
#include "VoronoiSimplexSolver.h"
#define SimplexSolverInterface VoronoiSimplexSolver

@ -30,7 +30,7 @@ subject to the following restrictions:
#include "quickprof.h"
#endif //USE_PROFILE
//iterative lcp and penalty method
/// SimpleConstraintSolver Sequentially applies impulses
float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
{
ContactSolverInfo info = infoGlobal;

@ -19,7 +19,9 @@ subject to the following restrictions:
#include "ConstraintSolver.h"
class IDebugDraw;
/// SimpleConstraintSolver uses a Propagation Method
/// SimpleConstraintSolver uses a Propagation Method and Sequentially applies impulses
/// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
/// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
/// Applies impulses for combined restitution and penetration recovery and to simulate friction
class SimpleConstraintSolver : public ConstraintSolver
{

@ -327,6 +327,10 @@ void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,flo
{
SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
if (local)
@ -345,6 +349,9 @@ void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,floa
if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
if (local)
{

@ -29,6 +29,7 @@ struct CcdConstructionInfo
: m_gravity(0,0,0),
m_mass(0.f),
m_restitution(0.1f),
m_friction(0.5f),
m_linearDamping(0.1f),
m_angularDamping(0.1f),
m_MotionState(0),

@ -679,8 +679,10 @@ void KX_ConvertODEEngineObject(KX_GameObject* gameobj,
#ifdef WIN32
#if _MSC_VER >= 1310
//only use SIMD Hull code under Win32
//#define TEST_HULL 1
#ifdef TEST_HULL
#define USE_HULL 1
//#define TEST_SIMD_HULL 1
#include "NarrowPhaseCollision/Hull.h"
#endif //#ifdef TEST_HULL

@ -142,6 +142,7 @@ void KX_TouchSensor::ReParent(SCA_IObject* parent)
KX_ClientObjectInfo *client_info = gameobj->getClientInfo();
client_info->m_gameobject = gameobj;
client_info->m_auxilary_info = NULL;
client_info->m_sensors.push_back(this);
SCA_ISensor::ReParent(parent);
}

@ -327,6 +327,10 @@ void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,flo
{
SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
if (local)
@ -345,6 +349,9 @@ void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,floa
if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
if (local)
{

@ -29,6 +29,7 @@ struct CcdConstructionInfo
: m_gravity(0,0,0),
m_mass(0.f),
m_restitution(0.1f),
m_friction(0.5f),
m_linearDamping(0.1f),
m_angularDamping(0.1f),
m_MotionState(0),