improved deactivation, exposed more tweakable parameters to python,

fixed some copy/paste bug in inertia/inverse inertia.
colors in deactivation debug
This commit is contained in:
Erwin Coumans 2005-08-04 19:07:39 +00:00
parent b12f801681
commit 210ab537ce
27 changed files with 548 additions and 180 deletions

@ -4,7 +4,6 @@
Version="8.00"
Name="Bullet3ContinuousCollision"
ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}"
SignManifests="true"
>
<Platforms>
<Platform
@ -277,14 +276,6 @@
RelativePath=".\NarrowPhaseCollision\ManifoldPoint.h"
>
</File>
<File
RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.cpp"
>
</File>
<File
RelativePath=".\NarrowPhaseCollision\ManifoldPointCollector.h"
>
</File>
<File
RelativePath=".\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cpp"
>
@ -437,6 +428,14 @@
RelativePath=".\CollisionShapes\TriangleCallback.h"
>
</File>
<File
RelativePath=".\CollisionShapes\TriangleMesh.cpp"
>
</File>
<File
RelativePath=".\CollisionShapes\TriangleMesh.h"
>
</File>
<File
RelativePath=".\CollisionShapes\TriangleMeshShape.cpp"
>

@ -38,3 +38,41 @@ void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3&
}
void BoxShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
float margin = 0.f;
SimdVector3 halfExtents = GetHalfExtents();
SimdScalar lx=2.f*(halfExtents.x());
SimdScalar ly=2.f*(halfExtents.y());
SimdScalar lz=2.f*(halfExtents.z());
inertia[0] = mass/(12.0f) * (ly*ly + lz*lz);
inertia[1] = mass/(12.0f) * (lx*lx + lz*lz);
inertia[2] = mass/(12.0f) * (lx*lx + ly*ly);
// float radius = GetHalfExtents().length();
// SimdScalar elem = 0.4f * mass * radius*radius;
// inertia[0] = inertia[1] = inertia[2] = elem;
return;
/*
float margin = GetMargin();
SimdVector3 halfExtents = GetHalfExtents();
SimdScalar lx=2.f*(halfExtents.x()+margin);
SimdScalar ly=2.f*(halfExtents.y()+margin);
SimdScalar lz=2.f*(halfExtents.z()+margin);
const SimdScalar x2 = lx*lx;
const SimdScalar y2 = ly*ly;
const SimdScalar z2 = lz*lz;
const SimdScalar scaledmass = mass * 0.08333333f;
inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
*/
// inertia.x() = scaledmass * (y2+z2);
// inertia.y() = scaledmass * (x2+z2);
// inertia.z() = scaledmass * (x2+y2);
}

@ -84,26 +84,7 @@ public:
virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
{
float margin = GetMargin();
SimdVector3 halfExtents = GetHalfExtents();
SimdScalar lx=2.f*(halfExtents.x()+margin);
SimdScalar ly=2.f*(halfExtents.y()+margin);
SimdScalar lz=2.f*(halfExtents.z()+margin);
const SimdScalar x2 = lx*lx;
const SimdScalar y2 = ly*ly;
const SimdScalar z2 = lz*lz;
const SimdScalar scaledmass = mass * 0.08333333f;
inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
// inertia.x() = scaledmass * (y2+z2);
// inertia.y() = scaledmass * (x2+z2);
// inertia.z() = scaledmass * (x2+y2);
}
virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia);
virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
{

@ -55,6 +55,10 @@ public:
m_cachedSeparatingAxis = seperatingAxis;
}
void SetPenetrationDepthSolver(ConvexPenetrationDepthSolver* penetrationDepthSolver)
{
m_penetrationDepthSolver = penetrationDepthSolver;
}
};
#endif //GJK_PAIR_DETECTOR_H

@ -14,7 +14,7 @@
#include "SimdTransform.h"
#include <assert.h>
float gContactBreakingTreshold = 0.02f;
float gContactBreakingTreshold = 0.002f;
PersistentManifold::PersistentManifold()
:m_body0(0),

@ -139,6 +139,11 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp
RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject;
RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject;
//todo: move this in the dispatcher
if ((convexbody->GetActivationState() == 2) &&(concavebody->GetActivationState() == 2))
return;
TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape();
if (m_convex.IsConvexShape())

@ -32,6 +32,8 @@
#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
#include "CollisionShapes/SphereShape.h"
#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
#ifdef WIN32
void DrawRasterizerLine(const float* from,const float* to,int color);
@ -50,22 +52,22 @@ bool gForceBoxBox = false;//false;//true;
bool gBoxBoxUseGjk = true;//true;//false;
bool gDisableConvexCollision = false;
bool gUseEpa = false;
ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
: CollisionAlgorithm(ci),
m_gjkPairDetector(0,0,&m_simplexSolver,&m_penetrationDepthSolver),
m_gjkPairDetector(0,0,&m_simplexSolver,0),
m_box0(*proxy0),
m_box1(*proxy1),
m_collisionImpulse(0.f),
m_ownManifold (false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false)
m_lowLevelOfDetail(false),
m_useEpa(gUseEpa)
{
CheckPenetrationDepthSolver();
RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
@ -127,6 +129,22 @@ public:
};
void ConvexConvexAlgorithm::CheckPenetrationDepthSolver()
{
// if (m_useEpa != gUseEpa)
{
m_useEpa = gUseEpa;
if (m_useEpa)
{
//not distributed
//m_gjkPairDetector.SetPenetrationDepthSolver(new Solid3EpaPenetrationDepth);
} else
{
m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
}
}
}
bool extra = false;
float gFriction = 0.5f;
@ -135,13 +153,19 @@ float gFriction = 0.5f;
//
void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount, bool useContinuous)
{
CheckPenetrationDepthSolver();
// printf("ConvexConvexAlgorithm::ProcessCollision\n");
m_collisionImpulse = 0.f;
RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
//todo: move this in the dispatcher
if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2))
return;
if (!m_manifoldPtr)
return;
@ -188,6 +212,8 @@ bool disableCcd = false;
float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,float timeStep,int stepCount)
{
CheckPenetrationDepthSolver();
m_collisionImpulse = 0.f;
RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
@ -222,7 +248,7 @@ float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad
//SubsimplexConvexCast ccd(&voronoiSimplex);
//GjkConvexCast ccd(&voronoiSimplex);
ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,&m_penetrationDepthSolver);
ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,m_penetrationDepthSolver);
if (disableCcd)
return 1.f;

@ -16,18 +16,16 @@
#include "NarrowPhaseCollision/PersistentManifold.h"
#include "BroadphaseCollision/BroadphaseProxy.h"
#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
class ConvexPenetrationDepthSolver;
///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
class ConvexConvexAlgorithm : public CollisionAlgorithm
{
//hardcoded penetration and simplex solver, its easy to make this flexible later
MinkowskiPenetrationDepthSolver m_penetrationDepthSolver;
ConvexPenetrationDepthSolver* m_penetrationDepthSolver;
VoronoiSimplexSolver m_simplexSolver;
GjkPairDetector m_gjkPairDetector;
bool m_useEpa;
public:
BroadphaseProxy m_box0;
BroadphaseProxy m_box1;
@ -36,6 +34,8 @@ public:
PersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
void CheckPenetrationDepthSolver();
public:

@ -142,18 +142,21 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in
for (i=0;i<islandmanifold.size();i++)
{
PersistentManifold* manifold = islandmanifold[i];
if (((RigidBody*)manifold->GetBody0()))
RigidBody* body0 = (RigidBody*)manifold->GetBody0();
RigidBody* body1 = (RigidBody*)manifold->GetBody1();
if (body0)
{
if ( ((RigidBody*)manifold->GetBody0())->GetActivationState() == ISLAND_SLEEPING)
if ( body0->GetActivationState() == ISLAND_SLEEPING)
{
((RigidBody*)manifold->GetBody0())->SetActivationState( WANTS_DEACTIVATION );//ACTIVE_TAG;
body0->SetActivationState( WANTS_DEACTIVATION);
}
}
if (((RigidBody*)manifold->GetBody1()))
if (body1)
{
if ( ((RigidBody*)manifold->GetBody1())->GetActivationState() == ISLAND_SLEEPING)
if ( body1->GetActivationState() == ISLAND_SLEEPING)
{
((RigidBody*)manifold->GetBody1())->SetActivationState(WANTS_DEACTIVATION);//ACTIVE_TAG;
body1->SetActivationState(WANTS_DEACTIVATION);
}
}

@ -97,18 +97,24 @@ typedef SimdScalar dQuaternion[4];
void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{
// q = (s,vx,vy,vz)
SimdScalar qq1 = 2*q[1]*q[1];
SimdScalar qq2 = 2*q[2]*q[2];
SimdScalar qq3 = 2*q[3]*q[3];
_R(0,0) = 1 - qq2 - qq3;
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 - qq1 - qq3;
_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 - qq1 - qq2;
_R(2,2) = 1.f - qq1 - qq2;
_R(2,3) = 0.f;
}
@ -131,8 +137,8 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
//convert data
body->m_facc.setValue(0,0,0);
body->m_tacc.setValue(0,0,0);
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++)
@ -147,9 +153,9 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1];
body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2];
body->m_I[0+0*4] = body->getInvInertiaDiagLocal()[0];
body->m_I[1+1*4] = body->getInvInertiaDiagLocal()[1];
body->m_I[2+2*4] = 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];
/*
@ -241,7 +247,7 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
}
assert (m_CurJoint < MAX_JOINTS_1);
if (manifold->GetContactPoint(i).GetDistance() < 0.f)
if (manifold->GetContactPoint(i).GetDistance() < 0.0f)
{
ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);

@ -420,7 +420,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
}
iMJ_ptr += 12;
J_ptr += 12;
Ad[i] = sor_w / (sum + cfm[i]);
Ad[i] = sor_w / sum;//(sum + cfm[i]);
}
// scale J and b by Ad
@ -434,7 +434,8 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
}
// scale Ad by CFM
for (i=0; i<m; i++) Ad[i] *= cfm[i];
for (i=0; i<m; i++)
Ad[i] *= cfm[i];
// order to solve constraint rows in
IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
@ -442,8 +443,12 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
#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;
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
@ -629,17 +634,18 @@ void SolveInternal1 (float global_cfm,
// 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]->tacc,-=,body[i]->avel,tmp);
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.
@ -742,9 +748,11 @@ void SolveInternal1 (float global_cfm,
// 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;
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;
for (j=0; j<3; j++)
tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1;
}
// put J*tmp1 into rhs
@ -756,7 +764,7 @@ void SolveInternal1 (float global_cfm,
// scale CFM
for (i=0; i<m; i++)
cfm[i] *= stepsize1;
cfm[i] =0.f;//*= stepsize1;
// load lambda from the value saved on the previous iteration
dRealAllocaArray (lambda,m);

@ -128,13 +128,20 @@ void ContactJoint::GetInfo2(Info2 *info)
depth = 0.f;
info->c[0] = k * depth;
info->cfm[0] = 0.f;
info->cfm[1] = 0.f;
info->cfm[2] = 0.f;
// set LCP limits for normal
info->lo[0] = 0;
info->hi[0] = 1e30f;//dInfinity;
info->lo[1] = 0;
info->hi[1] = 0.f;
info->lo[2] = 0.f;
info->hi[2] = 0.f;
#define DO_THE_FRICTION_2
#ifdef DO_THE_FRICTION_2
// now do jacobian for tangential forces
@ -151,7 +158,7 @@ void ContactJoint::GetInfo2(Info2 *info)
c2[2] = ccc2[2];
float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
float friction = 30.f;//0.01f;//FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction();
// first friction direction
if (m_numRows >= 2)
@ -182,13 +189,16 @@ void ContactJoint::GetInfo2(Info2 *info)
info->lo[1] = -friction;//-j->contact.surface.mu;
info->hi[1] = friction;//j->contact.surface.mu;
if (0)//j->contact.surface.mode & dContactApprox1_1)
if (1)//j->contact.surface.mode & dContactApprox1_1)
info->findex[1] = 0;
// set slip (constraint force mixing)
if (0)//j->contact.surface.mode & dContactSlip1)
{
// info->cfm[1] = j->contact.surface.slip1;
} else
{
info->cfm[1] = 0.f;
}
}

@ -4,12 +4,14 @@
#include "GEN_MinMax.h"
#include <SimdTransformUtil.h>
float gRigidBodyDamping = 0.5f;
float gLinearAirDamping = 1.f;
static int uniqueId = 0;
RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
: m_collisionShape(0),
m_activationState1(1),
m_deactivationTime(0.f),
m_hitFraction(1.f),
m_gravity(0.0f, 0.0f, 0.0f),
m_linearDamping(0.f),
@ -90,12 +92,13 @@ void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
#include <stdio.h>
void RigidBody::applyForces(SimdScalar step)
{
applyCentralForce(m_gravity);
m_linearVelocity *= GEN_clamped((1.f - step * m_linearDamping), 0.0f, 1.0f);
m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
}
@ -150,7 +153,11 @@ SimdQuaternion RigidBody::getOrientation() const
void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
{
m_worldTransform = xform;
SimdQuaternion orn;
// m_worldTransform.getBasis().getRotation(orn);
// orn.normalize();
// m_worldTransform.setBasis(SimdMatrix3x3(orn));
// m_worldTransform.getBasis().getRotation(m_orn1);
updateInertiaTensor();
}

@ -9,6 +9,9 @@ class CollisionShape;
struct MassProps;
typedef SimdScalar dMatrix3[4*3];
extern float gLinearAirDamping;
extern bool gUseEpa;
/// RigidBody class for RigidBody Dynamics
///
@ -170,7 +173,10 @@ public:
dMatrix3 m_invI;
int m_islandTag1;//temp
int m_activationState1;//temp
float m_deactivationTime;
int m_odeTag;
float m_padding[1024];
SimdVector3 m_tacc;//temp
SimdVector3 m_facc;
SimdScalar m_hitFraction; //time of impact calculation

@ -7,7 +7,11 @@
class BP_Proxy;
bool gEnableSleeping = false;//false;//true;
//'temporarily' global variables
float gDeactivationTime = 2.f;
float gLinearSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.0f;
#include "Dynamics/MassProps.h"
SimdVector3 startVel(0,0,0);//-10000);
@ -15,8 +19,7 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
{
m_collisionDelay = 0;
m_sleepingCounter = 0;
m_MotionState = ci.m_MotionState;
@ -190,36 +193,36 @@ void CcdPhysicsController::setNewClientInfo(void* clientinfo)
}
#ifdef WIN32
float gSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.f;
#else
void CcdPhysicsController::UpdateDeactivation(float timeStep)
{
if ( (m_body->GetActivationState() == 2))
return;
float gSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.0f;
#endif
if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
(m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
{
m_body->m_deactivationTime += timeStep;
} else
{
m_body->m_deactivationTime=0.f;
m_body->SetActivationState(0);
}
}
bool CcdPhysicsController::wantsSleeping()
{
if (!gEnableSleeping)
//disable deactivation
if (gDeactivationTime == 0.f)
return false;
if ( (m_body->GetActivationState() == 3) || (m_body->GetActivationState() == 2))
//2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION
if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3))
return true;
if ((m_body->getLinearVelocity().length2() < gSleepingTreshold*gSleepingTreshold) &&
(m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
{
m_sleepingCounter++;
} else
{
m_sleepingCounter=0;
}
if (m_sleepingCounter> 150)
if (m_body->m_deactivationTime> gDeactivationTime)
{
return true;
}

@ -10,6 +10,11 @@
#include "SimdScalar.h"
class CollisionShape;
extern float gDeactivationTime;
extern float gLinearSleepingTreshold;
extern float gAngularSleepingTreshold;
struct CcdConstructionInfo
{
CcdConstructionInfo()
@ -47,7 +52,6 @@ class CcdPhysicsController : public PHY_IPhysicsController
class PHY_IMotionState* m_MotionState;
CollisionShape* m_collisionShape;
int m_sleepingCounter;
public:
int m_collisionDelay;
@ -121,6 +125,8 @@ class CcdPhysicsController : public PHY_IPhysicsController
bool wantsSleeping();
void UpdateDeactivation(float timeStep);
void SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax);

@ -4,7 +4,7 @@
#include <algorithm>
#include "SimdTransform.h"
#include "Dynamics/RigidBody.h"
#include "BroadphaseCollision/BroadPhaseInterface.h"
#include "BroadphaseCollision/BroadphaseInterface.h"
#include "BroadphaseCollision/SimpleBroadphase.h"
#include "CollisionShapes/ConvexShape.h"
@ -33,8 +33,6 @@ bool useIslands = true;
//#include "BroadphaseCollision/QueryBox.h"
//todo: change this to allow dynamic registration of types!
unsigned long gNumIterations = 10;
#ifdef WIN32
void DrawRasterizerLine(const float* from,const float* to,int color);
#endif
@ -88,14 +86,17 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV
CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp)
:m_dispatcher(dispatcher),
m_broadphase(bp),
m_scalingPropagated(false)
m_scalingPropagated(false),
m_numIterations(30),
m_ccdMode(0),
m_solverType(-1)
{
if (!m_dispatcher)
{
OdeConstraintSolver* solver = new OdeConstraintSolver();
//SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
setSolverType(0);
}
if (!m_broadphase)
{
m_broadphase = new SimpleBroadphase();
@ -290,7 +291,6 @@ void CcdPhysicsEnvironment::UpdateActivationState()
}
bool gPredictCollision = false;//true;//false;
/// Perform an integration step of duration 'timeStep'.
@ -346,7 +346,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
int numsubstep = gNumIterations;
int numsubstep = m_numIterations;
DispatcherInfo dispatchInfo;
@ -367,7 +367,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
//contacts
m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies,m_debugDrawer);
m_dispatcher->SolveConstraints(timeStep, m_numIterations ,numRigidBodies,m_debugDrawer);
for (int g=0;g<numsubstep;g++)
{
@ -439,14 +439,41 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
#ifdef WIN32
SimdVector3 color (1,0,0);
if (m_debugDrawer)
{
//draw aabb
switch (body->GetActivationState())
{
case ISLAND_SLEEPING:
{
color.setValue(0,1,0);
break;
}
case WANTS_DEACTIVATION:
{
color.setValue(0,0,1);
break;
}
case ACTIVE_TAG:
{
break;
}
};
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}
#endif
scene->SetAabb(bp,minAabb,maxAabb);
}
}
@ -454,7 +481,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
if (gPredictCollision)
if (m_ccdMode == 3)
{
DispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
@ -505,6 +532,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep);
if (ctrl->wantsSleeping())
{
@ -543,26 +573,103 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
void CcdPhysicsEnvironment::setDebugMode(int debugMode)
{
if (debugMode > 10)
{
if (m_dispatcher)
delete m_dispatcher;
if (debugMode == 11)
{
SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
} else
{
OdeConstraintSolver* solver = new OdeConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
}
}
if (m_debugDrawer){
m_debugDrawer->SetDebugMode(debugMode);
}
}
void CcdPhysicsEnvironment::setNumIterations(int numIter)
{
m_numIterations = numIter;
}
void CcdPhysicsEnvironment::setDeactivationTime(float dTime)
{
gDeactivationTime = dTime;
}
void CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh)
{
gLinearSleepingTreshold = linTresh;
}
void CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh)
{
gAngularSleepingTreshold = angTresh;
}
void CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold)
{
gContactBreakingTreshold = contactBreakingTreshold;
}
void CcdPhysicsEnvironment::setCcdMode(int ccdMode)
{
m_ccdMode = ccdMode;
}
void CcdPhysicsEnvironment::setSolverSorConstant(float sor)
{
m_dispatcher->SetSor(sor);
}
void CcdPhysicsEnvironment::setSolverTau(float tau)
{
m_dispatcher->SetTau(tau);
}
void CcdPhysicsEnvironment::setSolverDamping(float damping)
{
m_dispatcher->SetDamping(damping);
}
void CcdPhysicsEnvironment::setLinearAirDamping(float damping)
{
gLinearAirDamping = damping;
}
void CcdPhysicsEnvironment::setUseEpa(bool epa)
{
gUseEpa = epa;
}
void CcdPhysicsEnvironment::setSolverType(int solverType)
{
switch (solverType)
{
case 1:
{
if (m_solverType != solverType)
{
if (m_dispatcher)
delete m_dispatcher;
SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
break;
}
}
case 0:
default:
if (m_solverType != solverType)
{
if (m_dispatcher)
delete m_dispatcher;
OdeConstraintSolver* solver= new OdeConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
break;
}
};
m_solverType = solverType ;
}
void CcdPhysicsEnvironment::SyncMotionStates(float timeStep)
{

@ -26,6 +26,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
SimdVector3 m_gravity;
BroadphaseInterface* m_broadphase;
IDebugDraw* m_debugDrawer;
int m_numIterations;
int m_ccdMode;
int m_solverType;
public:
CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
@ -43,6 +46,18 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
m_debugDrawer = debugDrawer;
}
virtual void setNumIterations(int numIter);
virtual void setDeactivationTime(float dTime);
virtual void setDeactivationLinearTreshold(float linTresh) ;
virtual void setDeactivationAngularTreshold(float angTresh) ;
virtual void setContactBreakingTreshold(float contactBreakingTreshold) ;
virtual void setCcdMode(int ccdMode);
virtual void setSolverType(int solverType);
virtual void setSolverSorConstant(float sor);
virtual void setSolverTau(float tau);
virtual void setSolverDamping(float damping);
virtual void setLinearAirDamping(float damping);
virtual void setUseEpa(bool epa) ;
virtual void beginFrame() {};
virtual void endFrame() {};

@ -99,7 +99,7 @@ class SimdQuadWord
SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z)
:m_x(x),m_y(y),m_z(z)
//todo, remove this in release/simd ?
,m_unusedW(1e30f)
,m_unusedW(0.f)
{
}

@ -917,7 +917,7 @@ void KX_ConvertBulletObject( class KX_GameObject* gameobj,
bm = new BoxShape(he);
bm->CalculateLocalInertia(ci.m_mass,ci.m_localInertiaTensor);
bm->SetMargin(0.05 * halfExtents.length());
break;
};
case KX_BOUNDCYLINDER:

@ -55,11 +55,14 @@ static char gPySetDeactivationTime__doc__[] = "setDeactivationTime(float time) T
static char gPySetDeactivationLinearTreshold__doc__[] = "setDeactivationLinearTreshold(float linearTreshold)";
static char gPySetDeactivationAngularTreshold__doc__[] = "setDeactivationAngularTreshold(float angularTreshold)";
static char gPySetContactBreakingTreshold__doc__[] = "setContactBreakingTreshold(float breakingTreshold) Reasonable default is 0.02 (if units are meters)";
static char gPySetCcdMode__doc__[] = "setCcdMode(int ccdMode) Very experimental, not recommended";
static char gPySetCcdMode__doc__[] = "setCcdMode(int ccdMode) Very experimental, not recommended";
static char gPySetSorConstant__doc__[] = "setSorConstant(float sor) Very experimental, not recommended";
static char gPySetTau__doc__[] = "setTau(float tau) Very experimental, not recommended";
static char gPySetDamping__doc__[] = "setDamping(float damping) Very experimental, not recommended";
static char gPySetSolverTau__doc__[] = "setTau(float tau) Very experimental, not recommended";
static char gPySetSolverDamping__doc__[] = "setDamping(float damping) Very experimental, not recommended";
static char gPySetLinearAirDamping__doc__[] = "setLinearAirDamping(float damping) Very experimental, not recommended";
static char gPySetUseEpa__doc__[] = "setUseEpa(int epa) Very experimental, not recommended";
static char gPySetSolverType__doc__[] = "setSolverType(int solverType) Very experimental, not recommended";
static char gPyCreateConstraint__doc__[] = "createConstraint(ob1,ob2,float restLength,float restitution,float damping)";
@ -210,7 +213,7 @@ static PyObject* gPySetSorConstant(PyObject* self,
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPySetTau(PyObject* self,
static PyObject* gPySetSolverTau(PyObject* self,
PyObject* args,
PyObject* kwds)
{
@ -219,14 +222,14 @@ static PyObject* gPySetTau(PyObject* self,
{
if (PHY_GetActiveEnvironment())
{
PHY_GetActiveEnvironment()->setTau( tau);
PHY_GetActiveEnvironment()->setSolverTau( tau);
}
}
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPySetDamping(PyObject* self,
static PyObject* gPySetSolverDamping(PyObject* self,
PyObject* args,
PyObject* kwds)
{
@ -235,12 +238,61 @@ static PyObject* gPySetDamping(PyObject* self,
{
if (PHY_GetActiveEnvironment())
{
PHY_GetActiveEnvironment()->setDamping( damping);
PHY_GetActiveEnvironment()->setSolverDamping( damping);
}
}
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPySetLinearAirDamping(PyObject* self,
PyObject* args,
PyObject* kwds)
{
float damping;
if (PyArg_ParseTuple(args,"f",&damping))
{
if (PHY_GetActiveEnvironment())
{
PHY_GetActiveEnvironment()->setLinearAirDamping( damping);
}
}
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPySetUseEpa(PyObject* self,
PyObject* args,
PyObject* kwds)
{
int epa;
if (PyArg_ParseTuple(args,"i",&epa))
{
if (PHY_GetActiveEnvironment())
{
PHY_GetActiveEnvironment()->setUseEpa(epa);
}
}
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPySetSolverType(PyObject* self,
PyObject* args,
PyObject* kwds)
{
int solverType;
if (PyArg_ParseTuple(args,"i",&solverType))
{
if (PHY_GetActiveEnvironment())
{
PHY_GetActiveEnvironment()->setSolverType(solverType);
}
}
Py_INCREF(Py_None); return Py_None;
}
static PyObject* gPyCreateConstraint(PyObject* self,
PyObject* args,
@ -337,10 +389,18 @@ static struct PyMethodDef physicsconstraints_methods[] = {
METH_VARARGS, gPySetCcdMode__doc__},
{"setSorConstant",(PyCFunction) gPySetSorConstant,
METH_VARARGS, gPySetSorConstant__doc__},
{"setTau",(PyCFunction) gPySetTau,
METH_VARARGS, gPySetTau__doc__},
{"setDamping",(PyCFunction) gPySetDamping,
METH_VARARGS, gPySetDamping__doc__},
{"setSolverTau",(PyCFunction) gPySetSolverTau,
METH_VARARGS, gPySetSolverTau__doc__},
{"setSolverDamping",(PyCFunction) gPySetSolverDamping,
METH_VARARGS, gPySetSolverDamping__doc__},
{"setLinearAirDamping",(PyCFunction) gPySetLinearAirDamping,
METH_VARARGS, gPySetLinearAirDamping__doc__},
{"setUseEpa",(PyCFunction) gPySetUseEpa,
METH_VARARGS, gPySetUseEpa__doc__},
{"setSolverType",(PyCFunction) gPySetSolverType,
METH_VARARGS, gPySetSolverType__doc__},

@ -8,7 +8,7 @@
class BP_Proxy;
//'temporarily' global variables
float gDeactivationTime = 0.f;
float gDeactivationTime = 2.f;
float gLinearSleepingTreshold = 0.8f;
float gAngularSleepingTreshold = 1.0f;
@ -19,8 +19,7 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
{
m_collisionDelay = 0;
m_sleepingCounter = 0;
m_MotionState = ci.m_MotionState;
@ -195,6 +194,23 @@ void CcdPhysicsController::setNewClientInfo(void* clientinfo)
}
void CcdPhysicsController::UpdateDeactivation(float timeStep)
{
if ( (m_body->GetActivationState() == 2))
return;
if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
(m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
{
m_body->m_deactivationTime += timeStep;
} else
{
m_body->m_deactivationTime=0.f;
m_body->SetActivationState(0);
}
}
bool CcdPhysicsController::wantsSleeping()
{
@ -202,20 +218,11 @@ bool CcdPhysicsController::wantsSleeping()
//disable deactivation
if (gDeactivationTime == 0.f)
return false;
if ( (m_body->GetActivationState() == 3) || (m_body->GetActivationState() == 2))
//2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION
if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3))
return true;
if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
(m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
{
m_sleepingCounter++;
} else
{
m_sleepingCounter=0;
}
if (m_sleepingCounter> gDeactivationTime)
if (m_body->m_deactivationTime> gDeactivationTime)
{
return true;
}

@ -52,7 +52,6 @@ class CcdPhysicsController : public PHY_IPhysicsController
class PHY_IMotionState* m_MotionState;
CollisionShape* m_collisionShape;
int m_sleepingCounter;
public:
int m_collisionDelay;
@ -126,6 +125,8 @@ class CcdPhysicsController : public PHY_IPhysicsController
bool wantsSleeping();
void UpdateDeactivation(float timeStep);
void SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax);

@ -88,15 +88,15 @@ CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,Br
m_broadphase(bp),
m_scalingPropagated(false),
m_numIterations(30),
m_ccdMode(0)
m_ccdMode(0),
m_solverType(-1)
{
if (!m_dispatcher)
{
OdeConstraintSolver* solver = new OdeConstraintSolver();
//SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
setSolverType(0);
}
if (!m_broadphase)
{
m_broadphase = new SimpleBroadphase();
@ -439,14 +439,41 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
#ifdef WIN32
SimdVector3 color (1,0,0);
if (m_debugDrawer)
{
//draw aabb
switch (body->GetActivationState())
{
case ISLAND_SLEEPING:
{
color.setValue(0,1,0);
break;
}
case WANTS_DEACTIVATION:
{
color.setValue(0,0,1);
break;
}
case ACTIVE_TAG:
{
break;
}
};
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}
#endif
scene->SetAabb(bp,minAabb,maxAabb);
}
}
@ -505,6 +532,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep);
if (ctrl->wantsSleeping())
{
@ -543,21 +573,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep)
void CcdPhysicsEnvironment::setDebugMode(int debugMode)
{
if (debugMode > 10)
{
if (m_dispatcher)
delete m_dispatcher;
if (debugMode > 100)
{
SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
} else
{
OdeConstraintSolver* solver = new OdeConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
}
}
if (m_debugDrawer){
m_debugDrawer->SetDebugMode(debugMode);
}
@ -597,16 +612,63 @@ void CcdPhysicsEnvironment::setSolverSorConstant(float sor)
m_dispatcher->SetSor(sor);
}
void CcdPhysicsEnvironment::setTau(float tau)
void CcdPhysicsEnvironment::setSolverTau(float tau)
{
m_dispatcher->SetTau(tau);
}
void CcdPhysicsEnvironment::setDamping(float damping)
void CcdPhysicsEnvironment::setSolverDamping(float damping)
{
m_dispatcher->SetDamping(damping);
}
void CcdPhysicsEnvironment::setLinearAirDamping(float damping)
{
gLinearAirDamping = damping;
}
void CcdPhysicsEnvironment::setUseEpa(bool epa)
{
gUseEpa = epa;
}
void CcdPhysicsEnvironment::setSolverType(int solverType)
{
switch (solverType)
{
case 1:
{
if (m_solverType != solverType)
{
if (m_dispatcher)
delete m_dispatcher;
SimpleConstraintSolver* solver= new SimpleConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
break;
}
}
case 0:
default:
if (m_solverType != solverType)
{
if (m_dispatcher)
delete m_dispatcher;
OdeConstraintSolver* solver= new OdeConstraintSolver();
m_dispatcher = new ToiContactDispatcher(solver);
break;
}
};
m_solverType = solverType ;
}
void CcdPhysicsEnvironment::SyncMotionStates(float timeStep)

@ -28,6 +28,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
IDebugDraw* m_debugDrawer;
int m_numIterations;
int m_ccdMode;
int m_solverType;
public:
CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0);
@ -51,10 +52,12 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
virtual void setDeactivationAngularTreshold(float angTresh) ;
virtual void setContactBreakingTreshold(float contactBreakingTreshold) ;
virtual void setCcdMode(int ccdMode);
virtual void setSolverType(int solverType);
virtual void setSolverSorConstant(float sor);
virtual void setTau(float tau);
virtual void setDamping(float damping);
virtual void setSolverTau(float tau);
virtual void setSolverDamping(float damping);
virtual void setLinearAirDamping(float damping);
virtual void setUseEpa(bool epa) ;
virtual void beginFrame() {};
virtual void endFrame() {};

@ -67,10 +67,16 @@ class PHY_IPhysicsEnvironment
virtual void setCcdMode(int ccdMode) {}
///successive overrelaxation constant, in case PSOR is used, values in between 1 and 2 guarantee converging behaviour
virtual void setSolverSorConstant(float sor) {}
///setSolverType, internal setting, chooses solvertype, PSOR, Dantzig, impulse based, penalty based
virtual void setSolverType(int solverType) {}
///setTau sets the spring constant of a penalty based solver
virtual void setTau(float tau) {}
virtual void setSolverTau(float tau) {}
///setDamping sets the damper constant of a penalty based solver
virtual void setDamping(float damping) {}
virtual void setSolverDamping(float damping) {}
///linear air damping for rigidbodies
virtual void setLinearAirDamping(float damping) {}
/// penetrationdepth setting
virtual void setUseEpa(bool epa) {}
virtual void setGravity(float x,float y,float z)=0;

@ -401,10 +401,15 @@ void RAS_OpenGLRasterizer::ClearCachingInfo(void)
void RAS_OpenGLRasterizer::EndFrame()
{
glDisable(GL_LIGHTING);
glDisable(GL_TEXTURE);
//DrawDebugLines
glBegin(GL_LINES);
for (unsigned int i=0;i<m_debugLines.size();i++)
{
glColor4f(m_debugLines[i].m_color[0],m_debugLines[i].m_color[1],m_debugLines[i].m_color[2],1.f);
const MT_Scalar* fromPtr = &m_debugLines[i].m_from.x();
const MT_Scalar* toPtr= &m_debugLines[i].m_to.x();