Refactoring: another huge number of changes, renamed methods to start with lower-case.
This commit is contained in:
@@ -31,7 +31,7 @@ public:
|
||||
|
||||
virtual ~btConstraintSolver() {}
|
||||
|
||||
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -100,8 +100,8 @@ float resolveSingleCollision(
|
||||
)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = contactPoint.GetPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.GetPositionWorldOnB();
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
|
||||
// printf("distance=%f\n",distance);
|
||||
@@ -136,7 +136,7 @@ float resolveSingleCollision(
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
|
||||
assert(cpd);
|
||||
|
||||
btScalar distance = cpd->m_penetration;//contactPoint.GetDistance();
|
||||
btScalar distance = cpd->m_penetration;//contactPoint.getDistance();
|
||||
|
||||
|
||||
//distance = 0.f;
|
||||
@@ -174,8 +174,8 @@ float resolveSingleFriction(
|
||||
)
|
||||
{
|
||||
|
||||
const btVector3& pos1 = contactPoint.GetPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.GetPositionWorldOnB();
|
||||
const btVector3& pos1 = contactPoint.getPositionWorldOnA();
|
||||
const btVector3& pos2 = contactPoint.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
|
||||
|
||||
@@ -46,7 +46,7 @@ btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody&
|
||||
}
|
||||
|
||||
|
||||
void btGeneric6DofConstraint::BuildJacobian()
|
||||
void btGeneric6DofConstraint::buildJacobian()
|
||||
{
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
@@ -116,7 +116,7 @@ void btGeneric6DofConstraint::BuildJacobian()
|
||||
}
|
||||
}
|
||||
|
||||
void btGeneric6DofConstraint::SolveConstraint(btScalar timeStep)
|
||||
void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
btScalar tau = 0.1f;
|
||||
btScalar damping = 1.0f;
|
||||
@@ -194,12 +194,12 @@ void btGeneric6DofConstraint::SolveConstraint(btScalar timeStep)
|
||||
}
|
||||
}
|
||||
|
||||
void btGeneric6DofConstraint::UpdateRHS(btScalar timeStep)
|
||||
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btScalar btGeneric6DofConstraint::ComputeAngle(int axis) const
|
||||
btScalar btGeneric6DofConstraint::computeAngle(int axis) const
|
||||
{
|
||||
btScalar angle;
|
||||
|
||||
|
||||
@@ -47,13 +47,13 @@ public:
|
||||
|
||||
btGeneric6DofConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void SolveConstraint(btScalar timeStep);
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void UpdateRHS(btScalar timeStep);
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
btScalar ComputeAngle(int axis) const;
|
||||
btScalar computeAngle(int axis) const;
|
||||
|
||||
void setLinearLowerLimit(const btVector3& linearLower)
|
||||
{
|
||||
@@ -99,11 +99,11 @@ public:
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
|
||||
const btRigidBody& GetRigidBodyA() const
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& GetRigidBodyB() const
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
@@ -45,7 +45,7 @@ m_angularOnly(false)
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::BuildJacobian()
|
||||
void btHingeConstraint::buildJacobian()
|
||||
{
|
||||
m_appliedImpulse = 0.f;
|
||||
|
||||
@@ -74,7 +74,7 @@ void btHingeConstraint::BuildJacobian()
|
||||
//these two jointAxis require equal angular velocities for both bodies
|
||||
|
||||
//this is unused for now, it's a todo
|
||||
btVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 jointAxis0;
|
||||
btVector3 jointAxis1;
|
||||
btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
|
||||
@@ -94,7 +94,7 @@ void btHingeConstraint::BuildJacobian()
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
void btHingeConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
//#define NEW_IMPLEMENTATION
|
||||
|
||||
@@ -150,8 +150,8 @@ void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
|
||||
// constraint axes in world space
|
||||
btVector3 jointAxis0;
|
||||
@@ -230,11 +230,11 @@ void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
btVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
|
||||
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
|
||||
|
||||
const btVector3& angVelA = GetRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = GetRigidBodyB().getAngularVelocity();
|
||||
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
|
||||
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
|
||||
btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
|
||||
btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
|
||||
btVector3 velrel = angA-angB;
|
||||
@@ -245,8 +245,8 @@ void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
if (len > 0.00001f)
|
||||
{
|
||||
btVector3 normal = velrel.normalized();
|
||||
float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
|
||||
GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
|
||||
float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal);
|
||||
// scale for mass and relaxation
|
||||
velrel *= (1.f/denom) * 0.9;
|
||||
}
|
||||
@@ -257,8 +257,8 @@ void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
if (len2>0.00001f)
|
||||
{
|
||||
btVector3 normal2 = angularError.normalized();
|
||||
float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
|
||||
GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
|
||||
float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
|
||||
getRigidBodyB().computeAngularImpulseDenominator(normal2);
|
||||
angularError *= (1.f/denom2) * relaxation;
|
||||
}
|
||||
|
||||
@@ -269,7 +269,7 @@ void btHingeConstraint::SolveConstraint(btScalar timeStep)
|
||||
|
||||
}
|
||||
|
||||
void btHingeConstraint::UpdateRHS(btScalar timeStep)
|
||||
void btHingeConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
@@ -46,17 +46,17 @@ public:
|
||||
|
||||
btHingeConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
virtual void buildJacobian();
|
||||
|
||||
virtual void SolveConstraint(btScalar timeStep);
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void UpdateRHS(btScalar timeStep);
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
const btRigidBody& GetRigidBodyA() const
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& GetRigidBodyB() const
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
@@ -38,7 +38,7 @@ btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::BuildJacobian()
|
||||
void btPoint2PointConstraint::buildJacobian()
|
||||
{
|
||||
m_appliedImpulse = 0.f;
|
||||
|
||||
@@ -62,7 +62,7 @@ void btPoint2PointConstraint::BuildJacobian()
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::SolveConstraint(btScalar timeStep)
|
||||
void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
@@ -109,7 +109,7 @@ void btPoint2PointConstraint::SolveConstraint(btScalar timeStep)
|
||||
}
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::UpdateRHS(btScalar timeStep)
|
||||
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
@@ -54,19 +54,19 @@ public:
|
||||
|
||||
btPoint2PointConstraint();
|
||||
|
||||
virtual void BuildJacobian();
|
||||
virtual void buildJacobian();
|
||||
|
||||
|
||||
virtual void SolveConstraint(btScalar timeStep);
|
||||
virtual void solveConstraint(btScalar timeStep);
|
||||
|
||||
void UpdateRHS(btScalar timeStep);
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
void SetPivotA(const btVector3& pivotA)
|
||||
void setPivotA(const btVector3& pivotA)
|
||||
{
|
||||
m_pivotInA = pivotA;
|
||||
}
|
||||
|
||||
void SetPivotB(const btVector3& pivotB)
|
||||
void setPivotB(const btVector3& pivotB)
|
||||
{
|
||||
m_pivotInB = pivotB;
|
||||
}
|
||||
|
||||
@@ -59,14 +59,14 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
}
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
float btSequentialImpulseConstraintSolver::SolveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
|
||||
int numiter = infoGlobal.m_numIterations;
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::beginBlock("Solve");
|
||||
btProfiler::beginBlock("solve");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
{
|
||||
@@ -76,8 +76,8 @@ float btSequentialImpulseConstraintSolver::SolveGroup(btPersistentManifold** man
|
||||
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);
|
||||
prepareConstraints(manifoldPtr[k],info,debugDrawer);
|
||||
solve(manifoldPtr[k],info,0,debugDrawer);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -93,14 +93,14 @@ float btSequentialImpulseConstraintSolver::SolveGroup(btPersistentManifold** man
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
|
||||
Solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
solve(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::endBlock("Solve");
|
||||
btProfiler::endBlock("solve");
|
||||
|
||||
btProfiler::beginBlock("SolveFriction");
|
||||
btProfiler::beginBlock("solveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
//now solve the friction
|
||||
@@ -112,11 +112,11 @@ float btSequentialImpulseConstraintSolver::SolveGroup(btPersistentManifold** man
|
||||
int k = j;
|
||||
if (i&1)
|
||||
k=numManifolds-j-1;
|
||||
SolveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
solveFriction(manifoldPtr[k],info,i,debugDrawer);
|
||||
}
|
||||
}
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::endBlock("SolveFriction");
|
||||
btProfiler::endBlock("solveFriction");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
return 0.f;
|
||||
@@ -131,29 +131,29 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
|
||||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
|
||||
void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
|
||||
|
||||
|
||||
//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
|
||||
{
|
||||
manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
|
||||
manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
|
||||
|
||||
int numpoints = manifoldPtr->GetNumContacts();
|
||||
int numpoints = manifoldPtr->getNumContacts();
|
||||
|
||||
gTotalContactPoints += numpoints;
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
{
|
||||
btManifoldPoint& cp = manifoldPtr->GetContactPoint(i);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
|
||||
if (cp.getDistance() <= 0.f)
|
||||
{
|
||||
const btVector3& pos1 = cp.GetPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.GetPositionWorldOnB();
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
|
||||
@@ -173,15 +173,15 @@ void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifol
|
||||
{
|
||||
//might be invalid
|
||||
cpd->m_persistentLifeTime++;
|
||||
if (cpd->m_persistentLifeTime != cp.GetLifeTime())
|
||||
if (cpd->m_persistentLifeTime != cp.getLifeTime())
|
||||
{
|
||||
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
|
||||
new (cpd) btConstraintPersistentData;
|
||||
cpd->m_persistentLifeTime = cp.GetLifeTime();
|
||||
cpd->m_persistentLifeTime = cp.getLifeTime();
|
||||
|
||||
} else
|
||||
{
|
||||
//printf("Persistent: cpd->m_persistentLifeTime = %i cp.GetLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.GetLifeTime());
|
||||
//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
|
||||
|
||||
}
|
||||
} else
|
||||
@@ -191,8 +191,8 @@ void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifol
|
||||
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());
|
||||
cpd->m_persistentLifeTime = cp.getLifeTime();
|
||||
//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
|
||||
|
||||
}
|
||||
assert(cpd);
|
||||
@@ -213,7 +213,7 @@ void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifol
|
||||
|
||||
float combinedRestitution = cp.m_combinedRestitution;
|
||||
|
||||
cpd->m_penetration = cp.GetDistance();
|
||||
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)
|
||||
@@ -248,14 +248,14 @@ void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifol
|
||||
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 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);
|
||||
denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
|
||||
denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
|
||||
denom = relaxation/(denom0+denom1);
|
||||
cpd->m_jacDiagABInvTangent1 = denom;
|
||||
|
||||
@@ -275,16 +275,16 @@ void btSequentialImpulseConstraintSolver::PrepareConstraints(btPersistentManifol
|
||||
}
|
||||
}
|
||||
|
||||
float btSequentialImpulseConstraintSolver::Solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
|
||||
|
||||
float maxImpulse = 0.f;
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
const int numpoints = manifoldPtr->getNumContacts();
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
@@ -296,14 +296,14 @@ float btSequentialImpulseConstraintSolver::Solve(btPersistentManifold* manifoldP
|
||||
else
|
||||
j=i;
|
||||
|
||||
btManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
btManifoldPoint& 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);
|
||||
debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
|
||||
}
|
||||
|
||||
{
|
||||
@@ -324,14 +324,14 @@ float btSequentialImpulseConstraintSolver::Solve(btPersistentManifold* manifoldP
|
||||
return maxImpulse;
|
||||
}
|
||||
|
||||
float btSequentialImpulseConstraintSolver::SolveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->GetBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->GetBody1();
|
||||
btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
|
||||
btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
|
||||
|
||||
|
||||
{
|
||||
const int numpoints = manifoldPtr->GetNumContacts();
|
||||
const int numpoints = manifoldPtr->getNumContacts();
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
for (int i=0;i<numpoints ;i++)
|
||||
@@ -341,8 +341,8 @@ float btSequentialImpulseConstraintSolver::SolveFriction(btPersistentManifold* m
|
||||
//if (iter % 2)
|
||||
// j = numpoints-1-i;
|
||||
|
||||
btManifoldPoint& cp = manifoldPtr->GetContactPoint(j);
|
||||
if (cp.GetDistance() <= 0.f)
|
||||
btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
|
||||
if (cp.getDistance() <= 0.f)
|
||||
{
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
|
||||
|
||||
@@ -29,9 +29,9 @@ class btIDebugDraw;
|
||||
/// Applies impulses for combined restitution and penetration recovery and to simulate friction
|
||||
class btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
float Solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
float SolveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
void PrepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
|
||||
float solve(btPersistentManifold* manifold, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
float solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
|
||||
void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
|
||||
|
||||
ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
|
||||
@@ -42,7 +42,7 @@ public:
|
||||
|
||||
///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
|
||||
///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
|
||||
void SetContactSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
void setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
|
||||
{
|
||||
m_contactDispatch[type0][type1] = func;
|
||||
}
|
||||
@@ -56,7 +56,7 @@ public:
|
||||
|
||||
virtual ~btSequentialImpulseConstraintSolver() {}
|
||||
|
||||
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -40,48 +40,48 @@ public:
|
||||
|
||||
btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB);
|
||||
|
||||
virtual void BuildJacobian() = 0;
|
||||
virtual void buildJacobian() = 0;
|
||||
|
||||
virtual void SolveConstraint(btScalar timeStep) = 0;
|
||||
virtual void solveConstraint(btScalar timeStep) = 0;
|
||||
|
||||
const btRigidBody& GetRigidBodyA() const
|
||||
const btRigidBody& getRigidBodyA() const
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
const btRigidBody& GetRigidBodyB() const
|
||||
const btRigidBody& getRigidBodyB() const
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
btRigidBody& GetRigidBodyA()
|
||||
btRigidBody& getRigidBodyA()
|
||||
{
|
||||
return m_rbA;
|
||||
}
|
||||
btRigidBody& GetRigidBodyB()
|
||||
btRigidBody& getRigidBodyB()
|
||||
{
|
||||
return m_rbB;
|
||||
}
|
||||
|
||||
int GetUserConstraintType() const
|
||||
int getUserConstraintType() const
|
||||
{
|
||||
return m_userConstraintType ;
|
||||
}
|
||||
|
||||
void SetUserConstraintType(int userConstraintType)
|
||||
void setUserConstraintType(int userConstraintType)
|
||||
{
|
||||
m_userConstraintType = userConstraintType;
|
||||
};
|
||||
|
||||
void SetUserConstraintId(int uid)
|
||||
void setUserConstraintId(int uid)
|
||||
{
|
||||
m_userConstraintId = uid;
|
||||
}
|
||||
|
||||
int GetUserConstraintId()
|
||||
int getUserConstraintId()
|
||||
{
|
||||
return m_userConstraintId;
|
||||
}
|
||||
float GetAppliedImpulse()
|
||||
float getAppliedImpulse()
|
||||
{
|
||||
return m_appliedImpulse;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user