Code-style consistency improvement:
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
@@ -11,7 +11,6 @@
|
||||
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
|
||||
#include "Bullet3Dynamics/shared/b3Inertia.h"
|
||||
|
||||
|
||||
struct b3CpuRigidBodyPipelineInternalData
|
||||
{
|
||||
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
|
||||
@@ -22,7 +21,6 @@ struct b3CpuRigidBodyPipelineInternalData
|
||||
b3CpuNarrowPhase* m_np;
|
||||
b3Config m_config;
|
||||
};
|
||||
|
||||
|
||||
b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
|
||||
{
|
||||
@@ -39,49 +37,43 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
|
||||
|
||||
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
|
||||
{
|
||||
|
||||
for (int i=0;i<this->getNumBodies();i++)
|
||||
for (int i = 0; i < this->getNumBodies(); i++)
|
||||
{
|
||||
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
|
||||
b3Float4 position = body->m_pos;
|
||||
b3Quat orientation = body->m_quat;
|
||||
b3Quat orientation = body->m_quat;
|
||||
|
||||
int collidableIndex = body->m_collidableIdx;
|
||||
b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
|
||||
int shapeIndex = collidable.m_shapeIndex;
|
||||
|
||||
if (shapeIndex>=0)
|
||||
{
|
||||
|
||||
|
||||
if (shapeIndex >= 0)
|
||||
{
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
|
||||
float margin=0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&worldAabb.m_minVec,&worldAabb.m_maxVec);
|
||||
m_data->m_bp->setAabb(i,worldAabb.m_minVec,worldAabb.m_maxVec,0);
|
||||
float margin = 0.f;
|
||||
b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
|
||||
m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
|
||||
void b3CpuRigidBodyPipeline::computeOverlappingPairs()
|
||||
{
|
||||
int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
m_data->m_bp->calculateOverlappingPairs();
|
||||
numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
|
||||
printf("numPairs=%d\n",numPairs);
|
||||
printf("numPairs=%d\n", numPairs);
|
||||
}
|
||||
|
||||
void b3CpuRigidBodyPipeline::computeContactPoints()
|
||||
{
|
||||
|
||||
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
|
||||
|
||||
m_data->m_np->computeContacts(pairs,m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
|
||||
|
||||
m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
|
||||
}
|
||||
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
{
|
||||
|
||||
//update world space aabb's
|
||||
updateAabbWorldSpace();
|
||||
|
||||
@@ -92,73 +84,71 @@ void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
|
||||
computeContactPoints();
|
||||
|
||||
//solve contacts
|
||||
|
||||
|
||||
//update transforms
|
||||
integrate(deltaTime);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
|
||||
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
|
||||
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
|
||||
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
|
||||
{
|
||||
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
|
||||
}
|
||||
|
||||
|
||||
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
|
||||
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
|
||||
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
|
||||
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
|
||||
{
|
||||
linear = -n;
|
||||
angular0 = -b3Cross(r0, n);
|
||||
angular1 = b3Cross(r1, n);
|
||||
}
|
||||
|
||||
|
||||
|
||||
static inline void b3SolveContact(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
static inline void b3SolveContact(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
{
|
||||
b3Vector3 dLinVelA;
|
||||
dLinVelA.setZero();
|
||||
b3Vector3 dAngVelA;
|
||||
dAngVelA.setZero();
|
||||
b3Vector3 dLinVelB;
|
||||
dLinVelB.setZero();
|
||||
b3Vector3 dAngVelB;
|
||||
dAngVelB.setZero();
|
||||
|
||||
b3Vector3 dLinVelA; dLinVelA.setZero();
|
||||
b3Vector3 dAngVelA; dAngVelA.setZero();
|
||||
b3Vector3 dLinVelB; dLinVelB.setZero();
|
||||
b3Vector3 dAngVelB; dAngVelB.setZero();
|
||||
|
||||
for(int ic=0; ic<4; ic++)
|
||||
for (int ic = 0; ic < 4; ic++)
|
||||
{
|
||||
// dont necessary because this makes change to 0
|
||||
if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
|
||||
if (cs.m_jacCoeffInv[ic] == 0.f) continue;
|
||||
|
||||
{
|
||||
b3Vector3 angular0, angular1, linear;
|
||||
b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
|
||||
b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
|
||||
b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
|
||||
b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
|
||||
|
||||
float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
|
||||
float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB) +
|
||||
cs.m_b[ic];
|
||||
rambdaDt *= cs.m_jacCoeffInv[ic];
|
||||
|
||||
{
|
||||
float prevSum = cs.m_appliedRambdaDt[ic];
|
||||
float updated = prevSum;
|
||||
updated += rambdaDt;
|
||||
updated = b3Max( updated, minRambdaDt[ic] );
|
||||
updated = b3Min( updated, maxRambdaDt[ic] );
|
||||
updated = b3Max(updated, minRambdaDt[ic]);
|
||||
updated = b3Min(updated, maxRambdaDt[ic]);
|
||||
rambdaDt = updated - prevSum;
|
||||
cs.m_appliedRambdaDt[ic] = updated;
|
||||
}
|
||||
|
||||
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
|
||||
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(linImp0.getX()));
|
||||
b3Assert(_finite(linImp0.getX()));
|
||||
b3Assert(_finite(linImp1.getX()));
|
||||
#endif
|
||||
{
|
||||
@@ -169,53 +159,46 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
static inline void b3SolveFriction(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
static inline void b3SolveFriction(b3ContactConstraint4& cs,
|
||||
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
|
||||
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
|
||||
float maxRambdaDt[4], float minRambdaDt[4])
|
||||
{
|
||||
|
||||
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
|
||||
if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
|
||||
const b3Vector3& center = (const b3Vector3&)cs.m_center;
|
||||
|
||||
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
|
||||
|
||||
b3Vector3 tangent[2];
|
||||
|
||||
b3PlaneSpace1 (n, tangent[0],tangent[1]);
|
||||
b3PlaneSpace1(n, tangent[0], tangent[1]);
|
||||
|
||||
b3Vector3 angular0, angular1, linear;
|
||||
b3Vector3 r0 = center - posA;
|
||||
b3Vector3 r1 = center - posB;
|
||||
for(int i=0; i<2; i++)
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
|
||||
b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
|
||||
float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
|
||||
linVelA, angVelA, linVelB, angVelB );
|
||||
linVelA, angVelA, linVelB, angVelB);
|
||||
rambdaDt *= cs.m_fJacCoeffInv[i];
|
||||
|
||||
{
|
||||
float prevSum = cs.m_fAppliedRambdaDt[i];
|
||||
float updated = prevSum;
|
||||
updated += rambdaDt;
|
||||
updated = b3Max( updated, minRambdaDt[i] );
|
||||
updated = b3Min( updated, maxRambdaDt[i] );
|
||||
rambdaDt = updated - prevSum;
|
||||
cs.m_fAppliedRambdaDt[i] = updated;
|
||||
}
|
||||
{
|
||||
float prevSum = cs.m_fAppliedRambdaDt[i];
|
||||
float updated = prevSum;
|
||||
updated += rambdaDt;
|
||||
updated = b3Max(updated, minRambdaDt[i]);
|
||||
updated = b3Min(updated, maxRambdaDt[i]);
|
||||
rambdaDt = updated - prevSum;
|
||||
cs.m_fAppliedRambdaDt[i] = updated;
|
||||
}
|
||||
|
||||
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
|
||||
b3Vector3 linImp0 = invMassA * linear * rambdaDt;
|
||||
b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
|
||||
b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
|
||||
b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
|
||||
#ifdef _WIN32
|
||||
b3Assert(_finite(linImp0.getX()));
|
||||
b3Assert(_finite(linImp1.getX()));
|
||||
@@ -226,57 +209,45 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
|
||||
angVelB += angImp1;
|
||||
}
|
||||
|
||||
{ // angular damping for point constraint
|
||||
b3Vector3 ab = ( posB - posA ).normalized();
|
||||
b3Vector3 ac = ( center - posA ).normalized();
|
||||
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
|
||||
{ // angular damping for point constraint
|
||||
b3Vector3 ab = (posB - posA).normalized();
|
||||
b3Vector3 ac = (center - posA).normalized();
|
||||
if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
|
||||
{
|
||||
float angNA = b3Dot( n, angVelA );
|
||||
float angNB = b3Dot( n, angVelB );
|
||||
float angNA = b3Dot(n, angVelA);
|
||||
float angNB = b3Dot(n, angVelB);
|
||||
|
||||
angVelA -= (angNA*0.1f)*n;
|
||||
angVelB -= (angNB*0.1f)*n;
|
||||
angVelA -= (angNA * 0.1f) * n;
|
||||
angVelB -= (angNB * 0.1f) * n;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
struct b3SolveTask// : public ThreadPool::Task
|
||||
struct b3SolveTask // : public ThreadPool::Task
|
||||
{
|
||||
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
|
||||
b3AlignedObjectArray<b3Inertia>& shapes,
|
||||
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
|
||||
b3AlignedObjectArray<b3Inertia>& shapes,
|
||||
b3AlignedObjectArray<b3ContactConstraint4>& constraints,
|
||||
int start, int nConstraints,
|
||||
int maxNumBatches,
|
||||
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
|
||||
)
|
||||
: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ),
|
||||
m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
|
||||
m_start( start ),
|
||||
m_nConstraints( nConstraints ),
|
||||
m_solveFriction( true ),
|
||||
m_maxNumBatches(maxNumBatches)
|
||||
{}
|
||||
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
|
||||
: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
|
||||
{
|
||||
}
|
||||
|
||||
unsigned short int getType(){ return 0; }
|
||||
unsigned short int getType() { return 0; }
|
||||
|
||||
void run(int tIdx)
|
||||
{
|
||||
b3AlignedObjectArray<int> usedBodies;
|
||||
//printf("run..............\n");
|
||||
|
||||
|
||||
for (int bb=0;bb<m_maxNumBatches;bb++)
|
||||
for (int bb = 0; bb < m_maxNumBatches; bb++)
|
||||
{
|
||||
usedBodies.resize(0);
|
||||
for(int ic=m_nConstraints-1; ic>=0; ic--)
|
||||
for (int ic = m_nConstraints - 1; ic >= 0; ic--)
|
||||
//for(int ic=0; ic<m_nConstraints; ic++)
|
||||
{
|
||||
|
||||
int i = m_start + ic;
|
||||
if (m_constraints[i].m_batchIdx != bb)
|
||||
continue;
|
||||
@@ -298,87 +269,80 @@ m_start( start ),
|
||||
//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
|
||||
}
|
||||
#endif
|
||||
if (aIdx==10)
|
||||
if (aIdx == 10)
|
||||
{
|
||||
//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
|
||||
}
|
||||
if (usedBodies.size()<(aIdx+1))
|
||||
if (usedBodies.size() < (aIdx + 1))
|
||||
{
|
||||
usedBodies.resize(aIdx+1,0);
|
||||
usedBodies.resize(aIdx + 1, 0);
|
||||
}
|
||||
|
||||
if (usedBodies.size()<(bIdx+1))
|
||||
|
||||
if (usedBodies.size() < (bIdx + 1))
|
||||
{
|
||||
usedBodies.resize(bIdx+1,0);
|
||||
usedBodies.resize(bIdx + 1, 0);
|
||||
}
|
||||
|
||||
if (bodyA.m_invMass)
|
||||
{
|
||||
b3Assert(usedBodies[aIdx]==0);
|
||||
b3Assert(usedBodies[aIdx] == 0);
|
||||
usedBodies[aIdx]++;
|
||||
}
|
||||
|
||||
|
||||
if (bodyB.m_invMass)
|
||||
{
|
||||
b3Assert(usedBodies[bIdx]==0);
|
||||
b3Assert(usedBodies[bIdx] == 0);
|
||||
usedBodies[bIdx]++;
|
||||
}
|
||||
|
||||
|
||||
if( !m_solveFriction )
|
||||
if (!m_solveFriction)
|
||||
{
|
||||
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
|
||||
|
||||
b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt );
|
||||
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
|
||||
|
||||
b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt);
|
||||
}
|
||||
else
|
||||
{
|
||||
float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
|
||||
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
|
||||
float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
|
||||
|
||||
float sum = 0;
|
||||
for(int j=0; j<4; j++)
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
sum +=m_constraints[i].m_appliedRambdaDt[j];
|
||||
sum += m_constraints[i].m_appliedRambdaDt[j];
|
||||
}
|
||||
frictionCoeff = 0.7f;
|
||||
for(int j=0; j<4; j++)
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
maxRambdaDt[j] = frictionCoeff*sum;
|
||||
maxRambdaDt[j] = frictionCoeff * sum;
|
||||
minRambdaDt[j] = -maxRambdaDt[j];
|
||||
}
|
||||
|
||||
b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt );
|
||||
|
||||
b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
|
||||
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
|
||||
maxRambdaDt, minRambdaDt);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_wgUsedBodies)
|
||||
{
|
||||
if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
|
||||
if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
|
||||
{
|
||||
m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
|
||||
}
|
||||
for (int i=0;i<usedBodies.size();i++)
|
||||
for (int i = 0; i < usedBodies.size(); i++)
|
||||
{
|
||||
if (usedBodies[i])
|
||||
{
|
||||
//printf("cell %d uses body %d\n", m_curWgidx,i);
|
||||
m_wgUsedBodies[m_curWgidx][i]=1;
|
||||
m_wgUsedBodies[m_curWgidx][i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
|
||||
@@ -397,24 +361,22 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
|
||||
int m_nIterations = 4;
|
||||
|
||||
b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
|
||||
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
|
||||
// const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
|
||||
int n = contactConstraints.size();
|
||||
//convert contacts...
|
||||
|
||||
|
||||
|
||||
int maxNumBatches = 250;
|
||||
|
||||
for(int iter=0; iter<m_nIterations; iter++)
|
||||
for (int iter = 0; iter < m_nIterations; iter++)
|
||||
{
|
||||
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
|
||||
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
|
||||
task.m_solveFriction = false;
|
||||
task.run(0);
|
||||
}
|
||||
|
||||
for(int iter=0; iter<m_nIterations; iter++)
|
||||
for (int iter = 0; iter < m_nIterations; iter++)
|
||||
{
|
||||
b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
|
||||
b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
|
||||
task.m_solveFriction = true;
|
||||
task.run(0);
|
||||
}
|
||||
@@ -422,53 +384,51 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
|
||||
|
||||
void b3CpuRigidBodyPipeline::integrate(float deltaTime)
|
||||
{
|
||||
float angDamping=0.f;
|
||||
b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
|
||||
float angDamping = 0.f;
|
||||
b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
|
||||
|
||||
//integrate transforms (external forces/gravity should be moved into constraint solver)
|
||||
for (int i=0;i<m_data->m_rigidBodies.size();i++)
|
||||
for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
|
||||
{
|
||||
b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
|
||||
b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
|
||||
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
|
||||
{
|
||||
b3RigidBodyData body;
|
||||
int bodyIndex = m_data->m_rigidBodies.size();
|
||||
body.m_invMass = mass ? 1.f/mass : 0.f;
|
||||
body.m_angVel.setValue(0,0,0);
|
||||
body.m_invMass = mass ? 1.f / mass : 0.f;
|
||||
body.m_angVel.setValue(0, 0, 0);
|
||||
body.m_collidableIdx = collidableIndex;
|
||||
body.m_frictionCoeff = 0.3f;
|
||||
body.m_linVel.setValue(0,0,0);
|
||||
body.m_pos.setValue(position[0],position[1],position[2]);
|
||||
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
|
||||
body.m_linVel.setValue(0, 0, 0);
|
||||
body.m_pos.setValue(position[0], position[1], position[2]);
|
||||
body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
|
||||
body.m_restituitionCoeff = 0.f;
|
||||
|
||||
m_data->m_rigidBodies.push_back(body);
|
||||
|
||||
|
||||
if (collidableIndex>=0)
|
||||
if (collidableIndex >= 0)
|
||||
{
|
||||
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
|
||||
|
||||
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
|
||||
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
|
||||
|
||||
b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
|
||||
b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
|
||||
|
||||
b3Scalar margin = 0.01f;
|
||||
b3Transform t;
|
||||
t.setIdentity();
|
||||
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
|
||||
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
|
||||
t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
|
||||
t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
|
||||
b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
|
||||
|
||||
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
|
||||
// b3Vector3 aabbMin,aabbMax;
|
||||
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
|
||||
} else
|
||||
m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
|
||||
// b3Vector3 aabbMin,aabbMax;
|
||||
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
|
||||
}
|
||||
else
|
||||
{
|
||||
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
|
||||
}
|
||||
@@ -476,13 +436,12 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
|
||||
return bodyIndex;
|
||||
}
|
||||
|
||||
|
||||
const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
|
||||
{
|
||||
return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
|
||||
}
|
||||
|
||||
int b3CpuRigidBodyPipeline::getNumBodies() const
|
||||
int b3CpuRigidBodyPipeline::getNumBodies() const
|
||||
{
|
||||
return m_data->m_rigidBodies.size();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user