mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-09-24 11:14:38 +00:00
First stage in refactoring Bullet: moved Bullet Collision and Dynamics and LinearMath into src folder, and all files in Collision Detection and Dynamics have bt prefix.
Made all buildsystems to work again (jam, msvc, cmake)
This commit is contained in:
ejcoumans
@@ -0,0 +1,361 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btSequentialImpulseConstraintSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btContactConstraint.h"
|
||||
#include "btSolve2LinearConstraint.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
#include "LinearMath/GenIDebugDraw.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "LinearMath/GenMinMax.h"
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
#include "LinearMath/GenQuickprof.h"
|
||||
#endif //USE_PROFILE
|
||||
|
||||
int totalCpd = 0;
|
||||
|
||||
int gTotalContactPoints = 0;
|
||||
|
||||
bool MyContactDestroyedCallback(void* userPersistentData)
|
||||
{
|
||||
assert (userPersistentData);
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*)userPersistentData;
|
||||
delete cpd;
|
||||
totalCpd--;
|
||||
//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver()
|
||||
{
|
||||
gContactDestroyedCallback = &MyContactDestroyedCallback;
|
||||
|
||||
//initialize default friction/contact funcs
|
||||
int i,j;
|
||||
for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
|
||||
for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
|
||||
{
|
||||
|
||||
m_contactDispatch[i][j] = resolveSingleCollision;
|
||||
m_frictionDispatch[i][j] = resolveSingleFriction;
|
||||
}
|
||||
}
|
||||
|
||||
/// SequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
float SequentialImpulseConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
ContactSolverInfo info = infoGlobal;
|
||||
|
||||
int numiter = infoGlobal.m_numIterations;
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::beginBlock("Solve");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
//interleaving the preparation with solving impacts the behaviour a lot, todo: find out why
|
||||
|
||||
PrepareConstraints(manifoldPtr[k],info,debugDrawer);
|
||||
Solve(manifoldPtr[k],info,0,debugDrawer);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int i;
|
||||
for ( i = 0;i<numiter-1;i++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k=j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
|
||||
Solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::endBlock("Solve");
|
||||
|
||||
Profiler::beginBlock("SolveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
//now solve the friction
|
||||
for (i = 0;i<numiter-1;i++)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numManifolds;j++)
|
||||
{
|
||||
int k = j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
SolveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
Profiler::endBlock("SolveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
|
||||
float penetrationResolveFactor = 0.9f;
|
||||
SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
|
||||
{
|
||||
SimdScalar rest = restitution * -rel_vel;
|
||||
return rest;
|
||||
}
|
||||
|
||||
|
||||
void SequentialImpulseConstraintSolver::PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
|
||||
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
|
||||
{
|
||||
manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
|
||||
|
||||
int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
gTotalContactPoints += numpoints;
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
const SimdVector3& pos1 = cp.GetPositionWorldOnA();
|
||||
const SimdVector3& pos2 = cp.GetPositionWorldOnB();
|
||||
|
||||
SimdVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
|
||||
SimdVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
|
||||
|
||||
|
||||
//this jacobian entry is re-used for all iterations
|
||||
JacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
|
||||
body1->getCenterOfMassTransform().getBasis().transpose(),
|
||||
rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
|
||||
body1->getInvInertiaDiagLocal(),body1->getInvMass());
|
||||
|
||||
|
||||
SimdScalar jacDiagAB = jac.getDiagonal();
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
if (cpd)
|
||||
{
|
||||
//might be invalid
|
||||
cpd->m_persistentLifeTime++;
|
||||
if (cpd->m_persistentLifeTime != cp.GetLifeTime())
|
||||
{
|
||||
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
new (cpd) ConstraintPersistentData;
|
||||
cpd->m_persistentLifeTime = cp.GetLifeTime();
|
||||
|
||||
} else
|
||||
{
|
||||
//printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
|
||||
cpd = new ConstraintPersistentData();
|
||||
totalCpd ++;
|
||||
//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
|
||||
cp.m_userPersistentData = cpd;
|
||||
cpd->m_persistentLifeTime = cp.GetLifeTime();
|
||||
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
|
||||
}
|
||||
assert(cpd);
|
||||
|
||||
cpd->m_jacDiagABInv = 1.f / jacDiagAB;
|
||||
|
||||
//Dependent on Rigidbody A and B types, fetch the contact/friction response func
|
||||
//perhaps do a similar thing for friction/restutution combiner funcs...
|
||||
|
||||
cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
|
||||
cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
|
||||
|
||||
SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
|
||||
SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
|
||||
SimdVector3 vel = vel1 - vel2;
|
||||
SimdScalar rel_vel;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
float combinedRestitution = cp.m_combinedRestitution;
|
||||
|
||||
cpd->m_penetration = cp.GetDistance();
|
||||
cpd->m_friction = cp.m_combinedFriction;
|
||||
cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
|
||||
if (cpd->m_restitution <= 0.) //0.f)
|
||||
{
|
||||
cpd->m_restitution = 0.0f;
|
||||
|
||||
};
|
||||
|
||||
//restitution and penetration work in same direction so
|
||||
//rel_vel
|
||||
|
||||
SimdScalar penVel = -cpd->m_penetration/info.m_timeStep;
|
||||
|
||||
if (cpd->m_restitution >= penVel)
|
||||
{
|
||||
cpd->m_penetration = 0.f;
|
||||
}
|
||||
|
||||
|
||||
float relaxation = info.m_damping;
|
||||
cpd->m_appliedImpulse *= relaxation;
|
||||
//for friction
|
||||
cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
|
||||
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
SimdPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
|
||||
|
||||
|
||||
#define NO_FRICTION_WARMSTART 1
|
||||
|
||||
#ifdef NO_FRICTION_WARMSTART
|
||||
cpd->m_accumulatedTangentImpulse0 = 0.f;
|
||||
cpd->m_accumulatedTangentImpulse1 = 0.f;
|
||||
#endif //NO_FRICTION_WARMSTART
|
||||
float denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
|
||||
float denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
|
||||
float denom = relaxation/(denom0+denom1);
|
||||
cpd->m_jacDiagABInvTangent0 = denom;
|
||||
|
||||
|
||||
denom0 = body0->ComputeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
|
||||
denom1 = body1->ComputeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
|
||||
denom = relaxation/(denom0+denom1);
|
||||
cpd->m_jacDiagABInvTangent1 = denom;
|
||||
|
||||
SimdVector3 totalImpulse =
|
||||
#ifndef NO_FRICTION_WARMSTART
|
||||
cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
|
||||
cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
|
||||
#endif //NO_FRICTION_WARMSTART
|
||||
cp.m_normalWorldOnB*cpd->m_appliedImpulse;
|
||||
|
||||
//apply previous frames impulse on both bodies
|
||||
body0->applyImpulse(totalImpulse, rel_pos1);
|
||||
body1->applyImpulse(-totalImpulse, rel_pos2);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
float maxImpulse = 0.f;
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
if (iter % 2)
|
||||
j = numpoints-1-i;
|
||||
else
|
||||
j=i;
|
||||
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
|
||||
if (iter == 0)
|
||||
{
|
||||
if (debugDrawer)
|
||||
debugDrawer->DrawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.GetDistance(),cp.GetLifeTime(),color);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
float impulse = cpd->m_contactSolverFunc(
|
||||
*body0,*body1,
|
||||
cp,
|
||||
info);
|
||||
|
||||
if (maxImpulse < impulse)
|
||||
maxImpulse = impulse;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return maxImpulse;
|
||||
}
|
||||
|
||||
float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer)
|
||||
{
|
||||
RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0();
|
||||
RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1();
|
||||
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
|
||||
SimdVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
|
||||
int j=i;
|
||||
//if (iter % 2)
|
||||
// j = numpoints-1-i;
|
||||
|
||||
ManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
{
|
||||
|
||||
ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData;
|
||||
cpd->m_frictionSolverFunc(
|
||||
*body0,*body1,
|
||||
cp,
|
||||
info);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
return 0.f;
|
||||
}
|
||||
Reference in New Issue
Block a user