mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-08 08:30:16 +00:00
improve handling of restitution by using the velocity (linear/angular) before applying forces: this is done by re-introducing the btSolverBody and only apply the forces to solver body, and use the original rigid body velocity for restitution computation.
warmstarting for contact points was broken, fix in btPersistentManifold enable split impulse by default (at the cost of some performance) add the option for zero-length friction (instead of recomputing friction directions using btPlaneSpace), use the solver mode flag SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS precompute lateral friction directions (in btManifoldResult) remove the mConstraintRow[3] from btManifoldPoint, it just took a lot of memory with no benefits: fixed it in btParallelConstraintSolver
This commit is contained in:
erwin.coumans
parent
37ebcc3aa6
commit
84b1774dda
@ -92,6 +92,10 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
|
||||
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
|
||||
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
|
||||
|
||||
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
|
||||
|
||||
|
||||
|
||||
//BP mod, store contact triangles.
|
||||
if (isSwapped)
|
||||
{
|
||||
|
||||
@ -76,9 +76,7 @@ class btManifoldPoint
|
||||
m_contactCFM2(0.f),
|
||||
m_lifeTime(0)
|
||||
{
|
||||
mConstraintRow[0].m_accumImpulse = 0.f;
|
||||
mConstraintRow[1].m_accumImpulse = 0.f;
|
||||
mConstraintRow[2].m_accumImpulse = 0.f;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -94,16 +92,16 @@ class btManifoldPoint
|
||||
btScalar m_combinedFriction;
|
||||
btScalar m_combinedRestitution;
|
||||
|
||||
//BP mod, store contact triangles.
|
||||
int m_partId0;
|
||||
int m_partId1;
|
||||
int m_index0;
|
||||
int m_index1;
|
||||
//BP mod, store contact triangles.
|
||||
int m_partId0;
|
||||
int m_partId1;
|
||||
int m_index0;
|
||||
int m_index1;
|
||||
|
||||
mutable void* m_userPersistentData;
|
||||
btScalar m_appliedImpulse;
|
||||
|
||||
bool m_lateralFrictionInitialized;
|
||||
|
||||
btScalar m_appliedImpulse;
|
||||
btScalar m_appliedImpulseLateral1;
|
||||
btScalar m_appliedImpulseLateral2;
|
||||
btScalar m_contactMotion1;
|
||||
@ -118,8 +116,6 @@ class btManifoldPoint
|
||||
|
||||
|
||||
|
||||
btConstraintRow mConstraintRow[3];
|
||||
|
||||
|
||||
btScalar getDistance() const
|
||||
{
|
||||
|
||||
@ -143,10 +143,6 @@ public:
|
||||
m_pointCache[index] = m_pointCache[lastUsedIndex];
|
||||
//get rid of duplicated userPersistentData pointer
|
||||
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
|
||||
m_pointCache[lastUsedIndex].mConstraintRow[0].m_accumImpulse = 0.f;
|
||||
m_pointCache[lastUsedIndex].mConstraintRow[1].m_accumImpulse = 0.f;
|
||||
m_pointCache[lastUsedIndex].mConstraintRow[2].m_accumImpulse = 0.f;
|
||||
|
||||
m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
|
||||
m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
|
||||
m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
|
||||
@ -164,9 +160,9 @@ public:
|
||||
#define MAINTAIN_PERSISTENCY 1
|
||||
#ifdef MAINTAIN_PERSISTENCY
|
||||
int lifeTime = m_pointCache[insertIndex].getLifeTime();
|
||||
btScalar appliedImpulse = m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse;
|
||||
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse;
|
||||
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse;
|
||||
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
|
||||
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
|
||||
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
|
||||
// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
|
||||
|
||||
|
||||
@ -181,9 +177,9 @@ public:
|
||||
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
|
||||
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
|
||||
|
||||
m_pointCache[insertIndex].mConstraintRow[0].m_accumImpulse = appliedImpulse;
|
||||
m_pointCache[insertIndex].mConstraintRow[1].m_accumImpulse = appliedLateralImpulse1;
|
||||
m_pointCache[insertIndex].mConstraintRow[2].m_accumImpulse = appliedLateralImpulse2;
|
||||
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
|
||||
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
|
||||
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
|
||||
|
||||
|
||||
m_pointCache[insertIndex].m_lifeTime = lifeTime;
|
||||
|
||||
@ -304,7 +304,7 @@ void btConeTwistConstraint::buildJacobian()
|
||||
|
||||
|
||||
|
||||
void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep)
|
||||
void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
#ifndef __SPU__
|
||||
if (m_useSolveConstraintObsolete)
|
||||
|
||||
@ -142,7 +142,7 @@ public:
|
||||
|
||||
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
|
||||
|
||||
virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep);
|
||||
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
|
||||
|
||||
void updateRHS(btScalar timeStep);
|
||||
|
||||
|
||||
@ -16,18 +16,20 @@ subject to the following restrictions:
|
||||
#ifndef BT_CONTACT_SOLVER_INFO
|
||||
#define BT_CONTACT_SOLVER_INFO
|
||||
|
||||
#include "LinearMath/btScalar.h"
|
||||
|
||||
enum btSolverMode
|
||||
{
|
||||
SOLVER_RANDMIZE_ORDER = 1,
|
||||
SOLVER_FRICTION_SEPARATE = 2,
|
||||
SOLVER_USE_WARMSTARTING = 4,
|
||||
SOLVER_USE_FRICTION_WARMSTARTING = 8,
|
||||
SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
|
||||
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
|
||||
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
|
||||
SOLVER_CACHE_FRIENDLY = 128,
|
||||
SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
|
||||
SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster.
|
||||
SOLVER_SIMD = 256,
|
||||
SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
|
||||
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
|
||||
};
|
||||
|
||||
struct btContactSolverInfoData
|
||||
@ -67,6 +69,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
|
||||
m_tau = btScalar(0.6);
|
||||
m_damping = btScalar(1.0);
|
||||
m_friction = btScalar(0.3);
|
||||
m_timeStep = btScalar(1.f/60.f);
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
@ -74,12 +77,12 @@ struct btContactSolverInfo : public btContactSolverInfoData
|
||||
m_erp2 = btScalar(0.1);
|
||||
m_globalCfm = btScalar(0.);
|
||||
m_sor = btScalar(1.);
|
||||
m_splitImpulse = false;
|
||||
m_splitImpulsePenetrationThreshold = -0.02f;
|
||||
m_splitImpulse = true;
|
||||
m_splitImpulsePenetrationThreshold = -.04f;
|
||||
m_linearSlop = btScalar(0.0);
|
||||
m_warmstartingFactor=btScalar(0.85);
|
||||
//m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
|
||||
m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
|
||||
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
|
||||
}
|
||||
};
|
||||
|
||||
@ -174,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
||||
|
||||
// current velocity difference
|
||||
|
||||
btVector3 angVelA;
|
||||
body0->internalGetAngularVelocity(angVelA);
|
||||
btVector3 angVelB;
|
||||
body1->internalGetAngularVelocity(angVelB);
|
||||
btVector3 angVelA = body0->getAngularVelocity();
|
||||
btVector3 angVelB = body1->getAngularVelocity();
|
||||
|
||||
btVector3 vel_diff;
|
||||
vel_diff = angVelA-angVelB;
|
||||
@ -225,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
|
||||
|
||||
btVector3 motorImp = clippedMotorImpulse * axis;
|
||||
|
||||
//body0->applyTorqueImpulse(motorImp);
|
||||
//body1->applyTorqueImpulse(-motorImp);
|
||||
|
||||
body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
|
||||
body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
|
||||
|
||||
body0->applyTorqueImpulse(motorImp);
|
||||
body1->applyTorqueImpulse(-motorImp);
|
||||
|
||||
return clippedMotorImpulse;
|
||||
|
||||
@ -292,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
|
||||
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
|
||||
|
||||
btVector3 vel1;
|
||||
body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
btVector3 vel2;
|
||||
body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar rel_vel = axis_normal_on_a.dot(vel);
|
||||
@ -348,16 +340,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
|
||||
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
|
||||
|
||||
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
|
||||
//body1.applyImpulse( impulse_vector, rel_pos1);
|
||||
//body2.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
|
||||
body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
|
||||
body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
|
||||
|
||||
|
||||
body1.applyImpulse( impulse_vector, rel_pos1);
|
||||
body2.applyImpulse(-impulse_vector, rel_pos2);
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
@ -16,8 +16,7 @@ subject to the following restrictions:
|
||||
#ifndef BT_JACOBIAN_ENTRY_H
|
||||
#define BT_JACOBIAN_ENTRY_H
|
||||
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
|
||||
|
||||
//notes:
|
||||
|
||||
@ -18,24 +18,23 @@ subject to the following restrictions:
|
||||
|
||||
#include "btSequentialImpulseConstraintSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "btContactConstraint.h"
|
||||
#include "btSolve2LinearConstraint.h"
|
||||
#include "btContactSolverInfo.h"
|
||||
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
#include "btJacobianEntry.h"
|
||||
//#include "btJacobianEntry.h"
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include <new>
|
||||
#include "LinearMath/btStackAlloc.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
//#include "btSolverBody.h"
|
||||
//#include "btSolverConstraint.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
#include <string.h> //for memset
|
||||
|
||||
int gNumSplitImpulseRecoveries = 0;
|
||||
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
:m_btSeed2(0)
|
||||
{
|
||||
@ -57,7 +56,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
|
||||
#endif//USE_SIMD
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
#ifdef USE_SIMD
|
||||
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
|
||||
@ -91,7 +90,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
@ -120,7 +119,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
||||
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
}
|
||||
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
#ifdef USE_SIMD
|
||||
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
|
||||
@ -151,7 +150,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
|
||||
@ -175,8 +174,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
const btSolverConstraint& c)
|
||||
{
|
||||
if (c.m_rhsPenetration)
|
||||
@ -203,7 +202,7 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
|
||||
}
|
||||
}
|
||||
|
||||
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
|
||||
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
#ifdef USE_SIMD
|
||||
if (!c.m_rhsPenetration)
|
||||
@ -277,9 +276,10 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
|
||||
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
|
||||
{
|
||||
|
||||
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
|
||||
|
||||
solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
@ -289,17 +289,27 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod
|
||||
|
||||
if (rb)
|
||||
{
|
||||
solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
|
||||
solverBody->m_worldTransform = rb->getWorldTransform();
|
||||
solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
|
||||
solverBody->m_originalBody = rb;
|
||||
solverBody->m_angularFactor = rb->getAngularFactor();
|
||||
solverBody->m_linearFactor = rb->getLinearFactor();
|
||||
solverBody->m_linearVelocity = rb->getLinearVelocity();
|
||||
solverBody->m_angularVelocity = rb->getAngularVelocity();
|
||||
} else
|
||||
{
|
||||
solverBody->internalGetInvMass().setValue(0,0,0);
|
||||
solverBody->m_worldTransform.setIdentity();
|
||||
solverBody->internalSetInvMass(btVector3(0,0,0));
|
||||
solverBody->m_originalBody = 0;
|
||||
solverBody->m_angularFactor.setValue(1,1,1);
|
||||
solverBody->m_linearFactor.setValue(1,1,1);
|
||||
solverBody->m_linearVelocity.setValue(0,0,0);
|
||||
solverBody->m_angularVelocity.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
@ -313,9 +323,11 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
|
||||
|
||||
|
||||
|
||||
void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
|
||||
void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
|
||||
static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
|
||||
{
|
||||
|
||||
|
||||
if (colObj && colObj->hasAnisotropicFriction())
|
||||
{
|
||||
// transform to local coordinates
|
||||
@ -326,20 +338,23 @@ void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec
|
||||
// ... and transform it back to global coordinates
|
||||
frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
|
||||
|
||||
btRigidBody* body0=btRigidBody::upcast(colObj0);
|
||||
btRigidBody* body1=btRigidBody::upcast(colObj1);
|
||||
|
||||
|
||||
solverConstraint.m_contactNormal = normalAxis;
|
||||
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
|
||||
solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
|
||||
btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
|
||||
btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
solverConstraint.m_originalContactPoint = 0;
|
||||
@ -392,11 +407,13 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
||||
|
||||
|
||||
{
|
||||
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
||||
@ -408,23 +425,24 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
|
||||
solverConstraint.m_cfm = cfmSlip;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
|
||||
{
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
|
||||
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
|
||||
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
|
||||
return solverConstraint;
|
||||
}
|
||||
|
||||
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
|
||||
{
|
||||
#if 0
|
||||
|
||||
int solverBodyIdA = -1;
|
||||
|
||||
if (body.getCompanionId() >= 0)
|
||||
@ -445,29 +463,33 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
|
||||
return 0;//assume first one is a fixed solver body
|
||||
}
|
||||
}
|
||||
|
||||
return solverBodyIdA;
|
||||
#endif
|
||||
return 0;
|
||||
|
||||
}
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
|
||||
btCollisionObject* colObj0, btCollisionObject* colObj1,
|
||||
int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
|
||||
btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2)
|
||||
{
|
||||
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
|
||||
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
btRigidBody* rb0 = bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = bodyB->m_originalBody;
|
||||
|
||||
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
||||
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
||||
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
||||
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
||||
rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
|
||||
rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
|
||||
|
||||
relaxation = 1.f;
|
||||
|
||||
@ -501,29 +523,27 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
||||
}
|
||||
|
||||
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
|
||||
solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
|
||||
solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
|
||||
|
||||
|
||||
|
||||
|
||||
btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
|
||||
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
vel = vel1 - vel2;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
|
||||
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
|
||||
btScalar restitution = 0.f;
|
||||
btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
|
||||
|
||||
{
|
||||
btVector3 vel1,vel2;
|
||||
|
||||
vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
|
||||
vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
|
||||
// btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
||||
vel = vel1 - vel2;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
|
||||
|
||||
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
|
||||
{
|
||||
restitution = 0.f;
|
||||
} else
|
||||
{
|
||||
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (restitution <= btScalar(0.))
|
||||
{
|
||||
@ -537,9 +557,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
||||
if (rb1)
|
||||
rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
@ -548,33 +568,41 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
|
||||
|
||||
rel_vel = vel1Dotn+vel2Dotn;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
|
||||
btScalar rel_vel = vel1Dotn+vel2Dotn;
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
btScalar velocityError = restitution - rel_vel;// * damping;
|
||||
|
||||
|
||||
btScalar erp = infoGlobal.m_erp2;
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
erp = infoGlobal.m_erp;
|
||||
}
|
||||
|
||||
if (penetration>0)
|
||||
{
|
||||
positionalError = 0;
|
||||
|
||||
velocityError -= penetration / infoGlobal.m_timeStep;
|
||||
} else
|
||||
{
|
||||
positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
|
||||
positionalError = -penetration * erp/infoGlobal.m_timeStep;
|
||||
}
|
||||
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
|
||||
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
//combine position and velocity into rhs
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_rhsPenetration = 0.f;
|
||||
|
||||
} else
|
||||
{
|
||||
//split position and velocity into rhs and m_rhsPenetration
|
||||
@ -594,51 +622,46 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
|
||||
btRigidBody* rb0, btRigidBody* rb1,
|
||||
int solverBodyIdA, int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
||||
{
|
||||
{
|
||||
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
|
||||
if (rb1)
|
||||
rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
|
||||
if (rb1)
|
||||
rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint2.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
||||
frictionConstraint1.m_appliedImpulse = 0.f;
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
|
||||
frictionConstraint2.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
btRigidBody* rb0 = bodyA->m_originalBody;
|
||||
btRigidBody* rb1 = bodyB->m_originalBody;
|
||||
|
||||
{
|
||||
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
|
||||
if (rb0)
|
||||
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
|
||||
if (rb1)
|
||||
bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
frictionConstraint2.m_appliedImpulse = 0.f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -651,12 +674,19 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
||||
colObj0 = (btCollisionObject*)manifold->getBody0();
|
||||
colObj1 = (btCollisionObject*)manifold->getBody1();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(*colObj0);
|
||||
int solverBodyIdB = getOrInitSolverBody(*colObj1);
|
||||
|
||||
btRigidBody* bodyA = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* bodyB = btRigidBody::upcast(colObj1);
|
||||
|
||||
btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
||||
btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
|
||||
|
||||
///avoid collision response between two static objects
|
||||
if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
|
||||
if (!solverBodyA || !solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody))
|
||||
return;
|
||||
|
||||
for (int j=0;j<manifold->getNumContacts();j++)
|
||||
@ -676,11 +706,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
|
||||
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
|
||||
solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
|
||||
solverConstraint.m_originalContactPoint = &cp;
|
||||
|
||||
setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
|
||||
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
|
||||
|
||||
// const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
// const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
@ -695,46 +726,47 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
||||
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
|
||||
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
|
||||
{
|
||||
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
|
||||
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
|
||||
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
|
||||
cp.m_lateralFrictionDir2.normalize();//??
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
cp.m_lateralFrictionInitialized = true;
|
||||
} else
|
||||
{
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
if (!(infoGlobal.m_solverMode & SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS))
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
}
|
||||
|
||||
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
|
||||
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
|
||||
cp.m_lateralFrictionInitialized = true;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
|
||||
}
|
||||
|
||||
setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
|
||||
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
|
||||
|
||||
}
|
||||
}
|
||||
@ -748,39 +780,34 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
m_maxOverrideNumSolverIterations = 0;
|
||||
|
||||
if (!(numConstraints + numManifolds))
|
||||
|
||||
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
// printf("empty\n");
|
||||
return 0.f;
|
||||
bodies[i]->setCompanionId(-1);
|
||||
}
|
||||
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body)
|
||||
{
|
||||
body->internalGetDeltaLinearVelocity().setZero();
|
||||
body->internalGetDeltaAngularVelocity().setZero();
|
||||
body->internalGetPushVelocity().setZero();
|
||||
body->internalGetTurnVelocity().setZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int i = 0; i < numBodies; i++)
|
||||
{
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body)
|
||||
{
|
||||
body->internalGetDeltaLinearVelocity().setZero();
|
||||
body->internalGetDeltaAngularVelocity().setZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
m_tmpSolverBodyPool.reserve(numBodies+1);
|
||||
m_tmpSolverBodyPool.resize(0);
|
||||
|
||||
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&fixedBody,0);
|
||||
|
||||
//convert all bodies
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
int bodyId = getOrInitSolverBody(*bodies[i]);
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
if (body && body->getInvMass())
|
||||
{
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
|
||||
solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
|
||||
solverBody.m_angularVelocity += body->getTotalTorque()*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
}
|
||||
}
|
||||
|
||||
if (1)
|
||||
{
|
||||
int j;
|
||||
@ -791,6 +818,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
constraint->internalSetAppliedImpulse(0.0f);
|
||||
}
|
||||
}
|
||||
|
||||
//btRigidBody* rb0=0,*rb1=0;
|
||||
|
||||
//if (1)
|
||||
@ -834,6 +862,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB);
|
||||
|
||||
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
|
||||
|
||||
|
||||
int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
|
||||
if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
|
||||
@ -848,16 +884,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
|
||||
currentConstraintRow[j].m_appliedImpulse = 0.f;
|
||||
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
|
||||
currentConstraintRow[j].m_solverBodyA = &rbA;
|
||||
currentConstraintRow[j].m_solverBodyB = &rbB;
|
||||
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
|
||||
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
|
||||
currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
|
||||
}
|
||||
|
||||
rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
|
||||
bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
|
||||
|
||||
|
||||
btTypedConstraint::btConstraintInfo2 info2;
|
||||
@ -967,7 +1006,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
|
||||
m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
|
||||
m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
|
||||
else
|
||||
m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
|
||||
|
||||
m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
|
||||
{
|
||||
int i;
|
||||
@ -996,12 +1039,12 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
|
||||
int j;
|
||||
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
|
||||
{
|
||||
if ((iteration & 7) == 0) {
|
||||
for (j=0; j<numNonContactPool; ++j) {
|
||||
|
||||
for (int j=0; j<numNonContactPool; ++j) {
|
||||
int tmp = m_orderNonContactConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
|
||||
@ -1011,14 +1054,14 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
//contact/friction constraints are not solved more than
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
{
|
||||
for (j=0; j<numConstraintPool; ++j) {
|
||||
for (int j=0; j<numConstraintPool; ++j) {
|
||||
int tmp = m_orderTmpConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
|
||||
m_orderTmpConstraintPool[swapi] = tmp;
|
||||
}
|
||||
|
||||
for (j=0; j<numFrictionPool; ++j) {
|
||||
for (int j=0; j<numFrictionPool; ++j) {
|
||||
int tmp = m_orderFrictionConstraintPool[j];
|
||||
int swapi = btRandInt2(j+1);
|
||||
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
|
||||
@ -1031,72 +1074,125 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
if (infoGlobal.m_solverMode & SOLVER_SIMD)
|
||||
{
|
||||
///solve all joint constraints, using SIMD, if available
|
||||
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
|
||||
}
|
||||
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
{
|
||||
for (j=0;j<numConstraints;j++)
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
|
||||
}
|
||||
|
||||
///solve all contact constraints using SIMD, if available
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
|
||||
|
||||
}
|
||||
|
||||
///solve all friction constraints, using SIMD, if available
|
||||
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
for (int c=0;c<numPoolConstraints;c++)
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
btScalar totalImpulse =0;
|
||||
|
||||
resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold);
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
totalImpulse = solveManifold.m_appliedImpulse;
|
||||
}
|
||||
bool applyFriction = true;
|
||||
if (applyFriction)
|
||||
{
|
||||
{
|
||||
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
|
||||
{
|
||||
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
|
||||
{
|
||||
//solve the friction constraints after all contact constraints, don't interleave them
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int j;
|
||||
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
|
||||
}
|
||||
|
||||
///solve all friction constraints, using SIMD, if available
|
||||
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
|
||||
//non-SIMD version
|
||||
///solve all joint constraints
|
||||
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
|
||||
if (iteration < constraint.m_overrideNumSolverIterations)
|
||||
resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
|
||||
}
|
||||
|
||||
if (iteration< infoGlobal.m_numIterations)
|
||||
{
|
||||
for (j=0;j<numConstraints;j++)
|
||||
for (int j=0;j<numConstraints;j++)
|
||||
{
|
||||
constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
|
||||
}
|
||||
///solve all contact constraints
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
for (int j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
|
||||
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
///solve all friction constraints
|
||||
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
for (int j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
@ -1106,7 +1202,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1131,7 +1227,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
|
||||
resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
|
||||
resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1147,7 +1243,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
|
||||
resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
|
||||
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1175,25 +1271,29 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int i,j;
|
||||
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
||||
// float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
// printf("pt->m_appliedImpulseLateral1 = %f\n", f);
|
||||
pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
||||
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
||||
}
|
||||
//do a callback here?
|
||||
}
|
||||
|
||||
//do a callback here?
|
||||
}
|
||||
|
||||
numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
|
||||
@ -1209,29 +1309,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
for ( i=0;i<numBodies;i++)
|
||||
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
||||
{
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
if (body)
|
||||
body->internalWritebackVelocity(infoGlobal.m_timeStep);
|
||||
{
|
||||
m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
|
||||
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
for ( i=0;i<numBodies;i++)
|
||||
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
||||
{
|
||||
btRigidBody* body = btRigidBody::upcast(bodies[i]);
|
||||
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
if (body)
|
||||
body->internalWritebackVelocity();
|
||||
{
|
||||
m_tmpSolverBodyPool[i].writebackVelocity();
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
|
||||
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
|
||||
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
|
||||
m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
|
||||
|
||||
m_tmpSolverBodyPool.resizeNoInitialize(0);
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
@ -1243,14 +1355,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
|
||||
|
||||
BT_PROFILE("solveGroup");
|
||||
//you need to provide at least some bodies
|
||||
btAssert(bodies);
|
||||
btAssert(numBodies);
|
||||
|
||||
|
||||
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
|
||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
|
||||
solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
@ -1260,10 +1370,4 @@ void btSequentialImpulseConstraintSolver::reset()
|
||||
m_btSeed2 = 0;
|
||||
}
|
||||
|
||||
btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
|
||||
{
|
||||
static btRigidBody s_fixed(0, 0,0);
|
||||
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
|
||||
return s_fixed;
|
||||
}
|
||||
|
||||
|
||||
@ -16,19 +16,23 @@ subject to the following restrictions:
|
||||
#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
#include "btConstraintSolver.h"
|
||||
class btIDebugDraw;
|
||||
#include "btContactConstraint.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
#include "btTypedConstraint.h"
|
||||
class btPersistentManifold;
|
||||
class btStackAlloc;
|
||||
class btDispatcher;
|
||||
class btCollisionObject;
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
|
||||
|
||||
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
|
||||
ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver
|
||||
{
|
||||
protected:
|
||||
|
||||
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
|
||||
btConstraintArray m_tmpSolverContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverNonContactConstraintPool;
|
||||
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
|
||||
@ -38,55 +42,54 @@ protected:
|
||||
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
|
||||
int m_maxOverrideNumSolverIterations;
|
||||
|
||||
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB,
|
||||
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
|
||||
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
|
||||
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
|
||||
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp,
|
||||
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
|
||||
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
|
||||
btVector3& rel_pos1, btVector3& rel_pos2);
|
||||
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1,
|
||||
void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
|
||||
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
|
||||
|
||||
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
|
||||
unsigned long m_btSeed2;
|
||||
|
||||
// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
|
||||
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
|
||||
|
||||
|
||||
void resolveSplitPenetrationSIMD(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btSolverBody& bodyA,btSolverBody& bodyB,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btRigidBody& body1,
|
||||
btRigidBody& body2,
|
||||
btSolverBody& bodyA,btSolverBody& bodyB,
|
||||
const btSolverConstraint& contactConstraint);
|
||||
|
||||
//internal method
|
||||
int getOrInitSolverBody(btCollisionObject& body);
|
||||
int getOrInitSolverBody(btCollisionObject& body);
|
||||
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
|
||||
|
||||
void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint);
|
||||
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
|
||||
|
||||
protected:
|
||||
static btRigidBody& getFixedBody();
|
||||
|
||||
|
||||
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
|
||||
btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
|
||||
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
|
||||
@ -122,9 +125,7 @@ public:
|
||||
|
||||
};
|
||||
|
||||
#ifndef BT_PREFER_SIMD
|
||||
typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
|
||||
|
||||
@ -19,7 +19,7 @@ subject to the following restrictions:
|
||||
class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
@ -105,22 +105,35 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2)
|
||||
#endif
|
||||
|
||||
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete
|
||||
ATTRIBUTE_ALIGNED64 (struct) btSolverBody
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
btTransform m_worldTransform;
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_angularFactor;
|
||||
btVector3 m_linearFactor;
|
||||
btVector3 m_invMass;
|
||||
btRigidBody* m_originalBody;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_angularVelocity;
|
||||
|
||||
btRigidBody* m_originalBody;
|
||||
void setWorldTransform(const btTransform& worldTransform)
|
||||
{
|
||||
m_worldTransform = worldTransform;
|
||||
}
|
||||
|
||||
const btTransform& getWorldTransform() const
|
||||
{
|
||||
return m_worldTransform;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
|
||||
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
|
||||
else
|
||||
velocity.setValue(0,0,0);
|
||||
}
|
||||
@ -128,7 +141,7 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete
|
||||
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
if (m_originalBody)
|
||||
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
|
||||
angVel =m_angularVelocity+m_deltaAngularVelocity;
|
||||
else
|
||||
angVel.setValue(0,0,0);
|
||||
}
|
||||
@ -137,9 +150,9 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
//if (m_invMass)
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
@ -148,36 +161,125 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude;
|
||||
m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
const btVector3& getDeltaLinearVelocity() const
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getDeltaAngularVelocity() const
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////
|
||||
///some internal methods, don't use them
|
||||
|
||||
btVector3& internalGetDeltaLinearVelocity()
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetDeltaAngularVelocity()
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& internalGetAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
const btVector3& internalGetInvMass() const
|
||||
{
|
||||
return m_invMass;
|
||||
}
|
||||
|
||||
void internalSetInvMass(const btVector3& invMass)
|
||||
{
|
||||
m_invMass = invMass;
|
||||
}
|
||||
|
||||
btVector3& internalGetPushVelocity()
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetTurnVelocity()
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
angVel = m_angularVelocity+m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
|
||||
m_linearVelocity +=m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity(btScalar timeStep)
|
||||
void writebackVelocityAndTransform(btScalar timeStep)
|
||||
{
|
||||
(void) timeStep;
|
||||
if (m_originalBody)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
|
||||
m_linearVelocity += m_deltaLinearVelocity;
|
||||
m_angularVelocity += m_deltaAngularVelocity;
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform newTransform;
|
||||
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
|
||||
m_originalBody->setWorldTransform(newTransform);
|
||||
|
||||
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
|
||||
{
|
||||
btQuaternion orn = m_worldTransform.getRotation();
|
||||
btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity,timeStep,newTransform);
|
||||
m_worldTransform = newTransform;
|
||||
}
|
||||
//m_worldTransform.setRotation(orn);
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
@ -20,6 +20,7 @@ class btRigidBody;
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "btJacobianEntry.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
//#define NO_FRICTION_TANGENTIALS 1
|
||||
#include "btSolverBody.h"
|
||||
@ -58,16 +59,10 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint
|
||||
int m_frictionIndex;
|
||||
btScalar m_unusedPadding1;
|
||||
};
|
||||
union
|
||||
{
|
||||
btRigidBody* m_solverBodyA;
|
||||
int m_companionIdA;
|
||||
};
|
||||
union
|
||||
{
|
||||
btRigidBody* m_solverBodyB;
|
||||
int m_companionIdB;
|
||||
};
|
||||
|
||||
int m_solverBodyIdA;
|
||||
|
||||
int m_solverBodyIdB;
|
||||
|
||||
union
|
||||
{
|
||||
|
||||
@ -134,11 +134,8 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
|
||||
{
|
||||
if (islandId<0)
|
||||
{
|
||||
if (numManifolds + m_numConstraints)
|
||||
{
|
||||
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
|
||||
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
}
|
||||
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
|
||||
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
} else
|
||||
{
|
||||
//also add all non-contact constraints/joints for this island
|
||||
@ -166,11 +163,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
|
||||
|
||||
if (m_solverInfo->m_minimumSolverBatchSize<=1)
|
||||
{
|
||||
///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
|
||||
if (numManifolds + numCurConstraints)
|
||||
{
|
||||
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
}
|
||||
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
} else
|
||||
{
|
||||
|
||||
@ -192,15 +185,12 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal
|
||||
}
|
||||
void processConstraints()
|
||||
{
|
||||
if (m_manifolds.size() + m_constraints.size()>0)
|
||||
{
|
||||
|
||||
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
|
||||
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
|
||||
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
|
||||
btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
|
||||
btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
|
||||
btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
|
||||
|
||||
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
}
|
||||
m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
|
||||
m_bodies.resize(0);
|
||||
m_manifolds.resize(0);
|
||||
m_constraints.resize(0);
|
||||
@ -977,7 +967,8 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
|
||||
btRigidBody* body = m_nonStaticRigidBodies[i];
|
||||
if (!body->isStaticOrKinematicObject())
|
||||
{
|
||||
body->integrateVelocities( timeStep);
|
||||
//don't integrate/update velocities here, it happens in the constraint solver
|
||||
|
||||
//damping
|
||||
body->applyDamping(timeStep);
|
||||
|
||||
|
||||
@ -250,7 +250,6 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btRigidBody::updateInertiaTensor()
|
||||
{
|
||||
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
|
||||
@ -317,26 +316,6 @@ bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const
|
||||
return true;
|
||||
}
|
||||
|
||||
void btRigidBody::internalWritebackVelocity(btScalar timeStep)
|
||||
{
|
||||
(void) timeStep;
|
||||
if (m_inverseMass)
|
||||
{
|
||||
setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform newTransform;
|
||||
btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
|
||||
setWorldTransform(newTransform);
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
// m_deltaLinearVelocity.setZero();
|
||||
// m_deltaAngularVelocity .setZero();
|
||||
// m_pushVelocity.setZero();
|
||||
// m_turnVelocity.setZero();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btRigidBody::addConstraintRef(btTypedConstraint* c)
|
||||
|
||||
@ -519,105 +519,7 @@ public:
|
||||
return m_rigidbodyFlags;
|
||||
}
|
||||
|
||||
const btVector3& getDeltaLinearVelocity() const
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getDeltaAngularVelocity() const
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getPushVelocity() const
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
const btVector3& getTurnVelocity() const
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////
|
||||
///some internal methods, don't use them
|
||||
|
||||
btVector3& internalGetDeltaLinearVelocity()
|
||||
{
|
||||
return m_deltaLinearVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetDeltaAngularVelocity()
|
||||
{
|
||||
return m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
const btVector3& internalGetAngularFactor() const
|
||||
{
|
||||
return m_angularFactor;
|
||||
}
|
||||
|
||||
const btVector3& internalGetInvMass() const
|
||||
{
|
||||
return m_invMass;
|
||||
}
|
||||
|
||||
btVector3& internalGetPushVelocity()
|
||||
{
|
||||
return m_pushVelocity;
|
||||
}
|
||||
|
||||
btVector3& internalGetTurnVelocity()
|
||||
{
|
||||
return m_turnVelocity;
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const
|
||||
{
|
||||
angVel = getAngularVelocity()+m_deltaAngularVelocity;
|
||||
}
|
||||
|
||||
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_inverseMass)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_inverseMass)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
void internalWritebackVelocity()
|
||||
{
|
||||
if (m_inverseMass)
|
||||
{
|
||||
setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
|
||||
setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
|
||||
//m_deltaLinearVelocity.setZero();
|
||||
//m_deltaAngularVelocity .setZero();
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void internalWritebackVelocity(btScalar timeStep);
|
||||
|
||||
|
||||
|
||||
///////////////////////////////////////////////
|
||||
|
||||
@ -19,7 +19,7 @@ subject to the following restrictions:
|
||||
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
|
||||
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
|
||||
#include "LinearMath/btPoolAllocator.h"
|
||||
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "BulletMultiThreaded/vectormath2bullet.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
@ -56,7 +56,6 @@ unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) + c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));
|
||||
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));
|
||||
|
||||
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
|
||||
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
|
||||
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
|
||||
@ -160,6 +159,7 @@ void CustomSolveConstraintsTaskParallel(
|
||||
const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,
|
||||
PfxConstraintPair *contactPairs,uint32_t numContactPairs,
|
||||
btPersistentManifold* offsetContactManifolds,
|
||||
btConstraintRow* offsetContactConstraintRows,
|
||||
const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,
|
||||
PfxConstraintPair *jointPairs,uint32_t numJointPairs,
|
||||
btSolverConstraint* offsetSolverConstraints,
|
||||
@ -222,8 +222,9 @@ void CustomSolveConstraintsTaskParallel(
|
||||
uint16_t iA = pfxGetRigidBodyIdA(pair);
|
||||
uint16_t iB = pfxGetRigidBodyIdB(pair);
|
||||
|
||||
btPersistentManifold& contact = offsetContactManifolds[pfxGetConstraintId1(pair)];
|
||||
|
||||
uint32_t contactIndex = pfxGetConstraintId1(pair);
|
||||
btPersistentManifold& contact = offsetContactManifolds[contactIndex];
|
||||
btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[contactIndex*12];
|
||||
|
||||
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
|
||||
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
|
||||
@ -235,9 +236,17 @@ void CustomSolveConstraintsTaskParallel(
|
||||
vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));
|
||||
vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));
|
||||
|
||||
for(int k=0;k<3;k++) {
|
||||
vmVector3 normal = btReadVector3(cp.mConstraintRow[k].m_normal);
|
||||
float deltaImpulse = cp.mConstraintRow[k].m_accumImpulse;
|
||||
float imp[3] =
|
||||
{
|
||||
cp.m_appliedImpulse,
|
||||
cp.m_appliedImpulseLateral1,
|
||||
cp.m_appliedImpulseLateral2
|
||||
};
|
||||
for(int k=0;k<3;k++)
|
||||
{
|
||||
vmVector3 normal = btReadVector3(contactConstraintRows[j*3+k].m_normal);
|
||||
contactConstraintRows[j*3+k].m_accumImpulse = imp[k];
|
||||
float deltaImpulse = contactConstraintRows[j*3+k].m_accumImpulse;
|
||||
solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;
|
||||
solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);
|
||||
solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;
|
||||
@ -246,9 +255,9 @@ void CustomSolveConstraintsTaskParallel(
|
||||
}
|
||||
else {
|
||||
btSolveContactConstraint(
|
||||
cp.mConstraintRow[0],
|
||||
cp.mConstraintRow[1],
|
||||
cp.mConstraintRow[2],
|
||||
contactConstraintRows[j*3],
|
||||
contactConstraintRows[j*3+1],
|
||||
contactConstraintRows[j*3+2],
|
||||
btReadVector3(cp.m_localPointA),
|
||||
btReadVector3(cp.m_localPointB),
|
||||
solverBodyA,
|
||||
@ -334,6 +343,11 @@ void btSetupContactConstraint(
|
||||
const TrbState &stateB,
|
||||
PfxSolverBody &solverBodyA,
|
||||
PfxSolverBody &solverBodyB,
|
||||
const vmVector3& linVelA,
|
||||
const vmVector3& angVelA,
|
||||
const vmVector3& linVelB,
|
||||
const vmVector3& angVelB,
|
||||
|
||||
float separateBias,
|
||||
float timeStep
|
||||
)
|
||||
@ -344,11 +358,17 @@ void btSetupContactConstraint(
|
||||
vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) -
|
||||
crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) -
|
||||
crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);
|
||||
|
||||
//use the velocities without the applied (gravity and external) forces for restitution computation
|
||||
vmVector3 vArestitution = linVelA + cross(angVelA,rA);
|
||||
vmVector3 vBrestitution = linVelB + cross(angVelB,rB);
|
||||
vmVector3 vABrestitution = vArestitution-vBrestitution;
|
||||
|
||||
vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
|
||||
vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
|
||||
vmVector3 vAB = vA-vB;
|
||||
|
||||
|
||||
vmVector3 tangent1,tangent2;
|
||||
btPlaneSpace1(contactNormal,tangent1,tangent2);
|
||||
|
||||
@ -404,6 +424,7 @@ void btSetupContactConstraint(
|
||||
void CustomSetupContactConstraintsTask(
|
||||
PfxConstraintPair *contactPairs,uint32_t numContactPairs,
|
||||
btPersistentManifold* offsetContactManifolds,
|
||||
btConstraintRow* offsetContactConstraintRows,
|
||||
TrbState *offsetRigStates,
|
||||
PfxSolverBody *offsetSolverBodies,
|
||||
uint32_t numRigidBodies,
|
||||
@ -422,7 +443,7 @@ void CustomSetupContactConstraintsTask(
|
||||
|
||||
int id = pfxGetConstraintId1(pair);
|
||||
btPersistentManifold& contact = offsetContactManifolds[id];
|
||||
|
||||
btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
|
||||
|
||||
TrbState &stateA = offsetRigStates[iA];
|
||||
// PfxRigBody &bodyA = offsetRigBodies[iA];
|
||||
@ -439,11 +460,40 @@ void CustomSetupContactConstraintsTask(
|
||||
|
||||
for(int j=0;j<contact.getNumContacts();j++) {
|
||||
btManifoldPoint& cp = contact.getContactPoint(j);
|
||||
|
||||
//pass the velocities without the applied (gravity and external) forces for restitution computation
|
||||
const btRigidBody* rbA = btRigidBody::upcast(contact.getBody0());
|
||||
const btRigidBody* rbB = btRigidBody::upcast(contact.getBody1());
|
||||
|
||||
btVector3 linVelA, linVelB;
|
||||
btVector3 angVelA, angVelB;
|
||||
|
||||
if (rbA && (rbA->getInvMass()>0.f))
|
||||
{
|
||||
linVelA = rbA->getLinearVelocity();
|
||||
angVelA = rbA->getAngularVelocity();
|
||||
} else
|
||||
{
|
||||
linVelA.setValue(0,0,0);
|
||||
angVelA.setValue(0,0,0);
|
||||
}
|
||||
|
||||
if (rbB && (rbB->getInvMass()>0.f))
|
||||
{
|
||||
linVelB = rbB->getLinearVelocity();
|
||||
angVelB = rbB->getAngularVelocity();
|
||||
} else
|
||||
{
|
||||
linVelB.setValue(0,0,0);
|
||||
angVelB.setValue(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
btSetupContactConstraint(
|
||||
cp.mConstraintRow[0],
|
||||
cp.mConstraintRow[1],
|
||||
cp.mConstraintRow[2],
|
||||
contactConstraintRows[j*3],
|
||||
contactConstraintRows[j*3+1],
|
||||
contactConstraintRows[j*3+2],
|
||||
cp.getDistance(),
|
||||
restitution,
|
||||
friction,
|
||||
@ -454,6 +504,8 @@ void CustomSetupContactConstraintsTask(
|
||||
stateB,
|
||||
solverBodyA,
|
||||
solverBodyB,
|
||||
(const vmVector3&)linVelA, (const vmVector3&)angVelA,
|
||||
(const vmVector3&)linVelB, (const vmVector3&)angVelB,
|
||||
separateBias,
|
||||
timeStep
|
||||
);
|
||||
@ -463,6 +515,36 @@ void CustomSetupContactConstraintsTask(
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void CustomWritebackContactConstraintsTask(
|
||||
PfxConstraintPair *contactPairs,uint32_t numContactPairs,
|
||||
btPersistentManifold* offsetContactManifolds,
|
||||
btConstraintRow* offsetContactConstraintRows,
|
||||
TrbState *offsetRigStates,
|
||||
PfxSolverBody *offsetSolverBodies,
|
||||
uint32_t numRigidBodies,
|
||||
float separateBias,
|
||||
float timeStep)
|
||||
{
|
||||
for(uint32_t i=0;i<numContactPairs;i++) {
|
||||
PfxConstraintPair &pair = contactPairs[i];
|
||||
if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||
|
||||
((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {
|
||||
continue;
|
||||
}
|
||||
int id = pfxGetConstraintId1(pair);
|
||||
btPersistentManifold& contact = offsetContactManifolds[id];
|
||||
btConstraintRow* contactConstraintRows = &offsetContactConstraintRows[id*12];
|
||||
for(int j=0;j<contact.getNumContacts();j++) {
|
||||
btManifoldPoint& cp = contact.getContactPoint(j);
|
||||
cp.m_appliedImpulse = contactConstraintRows[j*3+0].m_accumImpulse;
|
||||
cp.m_appliedImpulseLateral1 = contactConstraintRows[j*3+1].m_accumImpulse;
|
||||
cp.m_appliedImpulseLateral2 = contactConstraintRows[j*3+2].m_accumImpulse;
|
||||
}
|
||||
//contact.setCompositeFriction(friction);
|
||||
}
|
||||
}
|
||||
|
||||
void SolverThreadFunc(void* userPtr,void* lsMemory)
|
||||
{
|
||||
btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);
|
||||
@ -479,6 +561,7 @@ void SolverThreadFunc(void* userPtr,void* lsMemory)
|
||||
io->solveConstraints.contactPairs,
|
||||
io->solveConstraints.numContactPairs,
|
||||
io->solveConstraints.offsetContactManifolds,
|
||||
io->solveConstraints.offsetContactConstraintRows,
|
||||
|
||||
io->solveConstraints.jointParallelGroup,
|
||||
io->solveConstraints.jointParallelBatches,
|
||||
@ -528,6 +611,49 @@ void SolverThreadFunc(void* userPtr,void* lsMemory)
|
||||
CustomSetupContactConstraintsTask(
|
||||
io->setupContactConstraints.offsetContactPairs+start,batch,
|
||||
io->setupContactConstraints.offsetContactManifolds,
|
||||
io->setupContactConstraints.offsetContactConstraintRows,
|
||||
io->setupContactConstraints.offsetRigStates,
|
||||
// io->setupContactConstraints.offsetRigBodies,
|
||||
io->setupContactConstraints.offsetSolverBodies,
|
||||
io->setupContactConstraints.numRigidBodies,
|
||||
io->setupContactConstraints.separateBias,
|
||||
io->setupContactConstraints.timeStep);
|
||||
}
|
||||
else {
|
||||
empty = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS:
|
||||
{
|
||||
bool empty = false;
|
||||
while(!empty) {
|
||||
int start,batch;
|
||||
|
||||
criticalsection->lock();
|
||||
|
||||
start = (int)criticalsection->getSharedParam(0);
|
||||
batch = (int)criticalsection->getSharedParam(1);
|
||||
|
||||
//PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);
|
||||
|
||||
// ŽŸ‚̃oƒbƒtƒ@‚ðƒZƒbƒg
|
||||
int nextStart = start + batch;
|
||||
int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);
|
||||
int nextBatch = (rest > batch)?batch:rest;
|
||||
|
||||
criticalsection->setSharedParam(0,nextStart);
|
||||
criticalsection->setSharedParam(1,nextBatch);
|
||||
|
||||
criticalsection->unlock();
|
||||
|
||||
if(batch > 0) {
|
||||
CustomWritebackContactConstraintsTask(
|
||||
io->setupContactConstraints.offsetContactPairs+start,batch,
|
||||
io->setupContactConstraints.offsetContactManifolds,
|
||||
io->setupContactConstraints.offsetContactConstraintRows,
|
||||
io->setupContactConstraints.offsetRigStates,
|
||||
// io->setupContactConstraints.offsetRigBodies,
|
||||
io->setupContactConstraints.offsetSolverBodies,
|
||||
@ -554,6 +680,7 @@ void SolverThreadFunc(void* userPtr,void* lsMemory)
|
||||
void CustomSetupContactConstraintsNew(
|
||||
PfxConstraintPair *contactPairs1,uint32_t numContactPairs,
|
||||
btPersistentManifold *offsetContactManifolds,
|
||||
btConstraintRow* offsetContactConstraintRows,
|
||||
TrbState *offsetRigStates,
|
||||
PfxSolverBody *offsetSolverBodies,
|
||||
uint32_t numRigidBodies,
|
||||
@ -561,7 +688,8 @@ void CustomSetupContactConstraintsNew(
|
||||
float timeStep,
|
||||
class btThreadSupportInterface* threadSupport,
|
||||
btCriticalSection* criticalSection,
|
||||
btConstraintSolverIO *io
|
||||
btConstraintSolverIO *io ,
|
||||
uint8_t cmd
|
||||
)
|
||||
{
|
||||
int maxTasks = threadSupport->getNumTasks();
|
||||
@ -584,11 +712,12 @@ void CustomSetupContactConstraintsNew(
|
||||
}
|
||||
|
||||
for(int t=0;t<maxTasks;t++) {
|
||||
io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS;
|
||||
io[t].cmd = cmd;
|
||||
io[t].setupContactConstraints.offsetContactPairs = contactPairs1;
|
||||
io[t].setupContactConstraints.numContactPairs1 = numContactPairs;
|
||||
io[t].setupContactConstraints.offsetRigStates = offsetRigStates;
|
||||
io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;
|
||||
io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;
|
||||
io[t].setupContactConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
|
||||
io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;
|
||||
io[t].setupContactConstraints.numRigidBodies = numRigidBodies;
|
||||
io[t].setupContactConstraints.separateBias = separationBias;
|
||||
@ -733,6 +862,7 @@ void CustomSolveConstraintsParallel(
|
||||
|
||||
PfxConstraintPair *jointPairs,uint32_t numJointPairs,
|
||||
btPersistentManifold* offsetContactManifolds,
|
||||
btConstraintRow* offsetContactConstraintRows,
|
||||
btSolverConstraint* offsetSolverConstraints,
|
||||
TrbState *offsetRigStates,
|
||||
PfxSolverBody *offsetSolverBodies,
|
||||
@ -780,9 +910,9 @@ void CustomSolveConstraintsParallel(
|
||||
io->solveConstraints.jointParallelBatches,
|
||||
io->solveConstraints.jointPairs,
|
||||
io->solveConstraints.numJointPairs,
|
||||
io->solveConstraints.offsetJoints,
|
||||
io->solveConstraints.offsetSolverConstraints,
|
||||
|
||||
io->solveConstraints.offsetRigStates,
|
||||
io->solveConstraints.offsetRigStates1,
|
||||
io->solveConstraints.offsetSolverBodies,
|
||||
io->solveConstraints.numRigidBodies,
|
||||
io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);
|
||||
@ -794,6 +924,7 @@ void CustomSolveConstraintsParallel(
|
||||
io[t].solveConstraints.contactPairs = contactPairs;
|
||||
io[t].solveConstraints.numContactPairs = numContactPairs;
|
||||
io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;
|
||||
io[t].solveConstraints.offsetContactConstraintRows = offsetContactConstraintRows;
|
||||
io[t].solveConstraints.jointParallelGroup = jgroup;
|
||||
io[t].solveConstraints.jointParallelBatches = jbatches;
|
||||
io[t].solveConstraints.jointPairs = jointPairs;
|
||||
@ -869,6 +1000,7 @@ void CustomSolveConstraintsParallel(
|
||||
|
||||
void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,
|
||||
btPersistentManifold* offsetContactManifolds,
|
||||
PfxConstraintRow* offsetContactConstraintRows,
|
||||
TrbState* states,int numRigidBodies,
|
||||
struct PfxSolverBody* solverBodies,
|
||||
PfxConstraintPair* jointPairs, unsigned int numJoints,
|
||||
@ -920,16 +1052,19 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
|
||||
separateBias,
|
||||
timeStep);
|
||||
#else
|
||||
|
||||
CustomSetupContactConstraintsNew(
|
||||
(PfxConstraintPair*)new_pairs1,new_num,
|
||||
offsetContactManifolds,
|
||||
offsetContactConstraintRows,
|
||||
states,
|
||||
solverBodies,
|
||||
numRigidBodies,
|
||||
separateBias,
|
||||
timeStep,
|
||||
solverThreadSupport,
|
||||
criticalSection,solverIO
|
||||
criticalSection,solverIO,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS
|
||||
);
|
||||
|
||||
#endif //SEQUENTIAL_SETUP
|
||||
@ -954,6 +1089,7 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
|
||||
(PfxConstraintPair*)new_pairs1,new_num,
|
||||
jointPairs,numJoints,
|
||||
offsetContactManifolds,
|
||||
offsetContactConstraintRows,
|
||||
offsetSolverConstraints,
|
||||
states,
|
||||
solverBodies,
|
||||
@ -968,6 +1104,24 @@ void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphase
|
||||
#endif //SEQUENTIAL
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("writeback appliedImpulses");
|
||||
|
||||
CustomSetupContactConstraintsNew(
|
||||
(PfxConstraintPair*)new_pairs1,new_num,
|
||||
offsetContactManifolds,
|
||||
offsetContactConstraintRows,
|
||||
states,
|
||||
solverBodies,
|
||||
numRigidBodies,
|
||||
separateBias,
|
||||
timeStep,
|
||||
solverThreadSupport,
|
||||
criticalSection,solverIO,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -977,6 +1131,7 @@ struct btParallelSolverMemoryCache
|
||||
btAlignedObjectArray<PfxSolverBody> m_mysolverbodies;
|
||||
btAlignedObjectArray<PfxBroadphasePair> m_mypairs;
|
||||
btAlignedObjectArray<PfxConstraintPair> m_jointPairs;
|
||||
btAlignedObjectArray<PfxConstraintRow> m_constraintRows;
|
||||
|
||||
};
|
||||
|
||||
@ -1057,9 +1212,12 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
//if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {
|
||||
if (rb && (rb->getInvMass()>0.f))
|
||||
{
|
||||
state.setAngularVelocity(vmVector3(rb->getAngularVelocity().getX(),rb->getAngularVelocity().getY(),rb->getAngularVelocity().getZ()));
|
||||
state.setLinearVelocity(vmVector3(rb->getLinearVelocity().getX(),rb->getLinearVelocity().getY(),rb->getLinearVelocity().getZ()));
|
||||
|
||||
btVector3 angVelPlusForces = rb->getAngularVelocity()+rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
|
||||
btVector3 linVelPlusForces = rb->getLinearVelocity()+rb->getTotalForce()*rb->getInvMass()*infoGlobal.m_timeStep;
|
||||
|
||||
state.setAngularVelocity((const vmVector3&)angVelPlusForces);
|
||||
state.setLinearVelocity((const vmVector3&) linVelPlusForces);
|
||||
|
||||
state.setMotionType(PfxMotionTypeActive);
|
||||
vmMatrix3 ori(solverBody.mOrientation);
|
||||
vmMatrix3 localInvInertia = vmMatrix3::identity();
|
||||
@ -1087,6 +1245,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
int totalPoints = 0;
|
||||
#ifndef USE_C_ARRAYS
|
||||
m_memoryCache->m_mypairs.resize(numManifolds);
|
||||
//4 points per manifold and 3 rows per point makes 12 rows per manifold
|
||||
m_memoryCache->m_constraintRows.resize(numManifolds*12);
|
||||
m_memoryCache->m_jointPairs.resize(numConstraints);
|
||||
#endif//USE_C_ARRAYS
|
||||
|
||||
@ -1196,7 +1356,10 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
|
||||
int idA = constraint->getRigidBodyA().getCompanionId();
|
||||
int idB = constraint->getRigidBodyB().getCompanionId();
|
||||
|
||||
|
||||
int j;
|
||||
for ( j=0;j<info1.m_numConstraintRows;j++)
|
||||
@ -1206,14 +1369,11 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
currentConstraintRow[j].m_upperLimit = FLT_MAX;
|
||||
currentConstraintRow[j].m_appliedImpulse = 0.f;
|
||||
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
|
||||
currentConstraintRow[j].m_solverBodyA = &rbA;
|
||||
currentConstraintRow[j].m_solverBodyB = &rbB;
|
||||
currentConstraintRow[j].m_solverBodyIdA = idA;
|
||||
currentConstraintRow[j].m_solverBodyIdB = idB;
|
||||
}
|
||||
|
||||
rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
|
||||
rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1236,8 +1396,6 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
constraints[i]->getInfo2(&info2);
|
||||
|
||||
|
||||
int idA = constraint->getRigidBodyA().getCompanionId();
|
||||
int idB = constraint->getRigidBodyB().getCompanionId();
|
||||
|
||||
|
||||
///finalize the constraint setup
|
||||
@ -1246,8 +1404,8 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
btSolverConstraint& solverConstraint = currentConstraintRow[j];
|
||||
solverConstraint.m_originalContactPoint = constraint;
|
||||
|
||||
solverConstraint.m_companionIdA = idA;
|
||||
solverConstraint.m_companionIdB = idB;
|
||||
solverConstraint.m_solverBodyIdA = idA;
|
||||
solverConstraint.m_solverBodyIdB = idB;
|
||||
|
||||
{
|
||||
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
|
||||
@ -1356,11 +1514,13 @@ btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int
|
||||
// PFX_PRINTF("num points PFX = %d\n",total);
|
||||
|
||||
|
||||
|
||||
PfxConstraintRow* contactRows = actualNumManifolds? &m_memoryCache->m_constraintRows[0] : 0;
|
||||
PfxBroadphasePair* actualPairs = m_memoryCache->m_mypairs.size() ? &m_memoryCache->m_mypairs[0] : 0;
|
||||
BPE_customConstraintSolverSequentialNew(
|
||||
actualNumManifolds,
|
||||
&m_memoryCache->m_mypairs[0],
|
||||
actualPairs,
|
||||
offsetContactManifolds,
|
||||
contactRows,
|
||||
&m_memoryCache->m_mystates[0],numRigidBodies,
|
||||
&m_memoryCache->m_mysolverbodies[0],
|
||||
jointPairs,actualNumJoints,
|
||||
|
||||
@ -188,6 +188,7 @@ class btPersistentManifold;
|
||||
enum {
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
|
||||
PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
|
||||
@ -198,6 +199,7 @@ struct PfxSetupContactConstraintsIO {
|
||||
PfxConstraintPair *offsetContactPairs;
|
||||
uint32_t numContactPairs1;
|
||||
btPersistentManifold* offsetContactManifolds;
|
||||
btConstraintRow* offsetContactConstraintRows;
|
||||
class TrbState *offsetRigStates;
|
||||
struct PfxSolverBody *offsetSolverBodies;
|
||||
uint32_t numRigidBodies;
|
||||
@ -214,6 +216,7 @@ struct PfxSolveConstraintsIO {
|
||||
PfxConstraintPair *contactPairs;
|
||||
uint32_t numContactPairs;
|
||||
btPersistentManifold *offsetContactManifolds;
|
||||
btConstraintRow* offsetContactConstraintRows;
|
||||
PfxParallelGroup *jointParallelGroup;
|
||||
PfxParallelBatch *jointParallelBatches;
|
||||
PfxConstraintPair *jointPairs;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user