diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp')
| -rw-r--r-- | thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp | 317 |
1 files changed, 138 insertions, 179 deletions
diff --git a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp index fbc84cc28d..f1080d9d5e 100644 --- a/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp +++ b/thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp @@ -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(); - - for(int ic=0; ic<4; ic++) + b3Vector3 dLinVelA; + dLinVelA.setZero(); + b3Vector3 dAngVelA; + dAngVelA.setZero(); + b3Vector3 dLinVelB; + dLinVelB.setZero(); + b3Vector3 dAngVelB; + dAngVelB.setZero(); + + 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) - {} - - unsigned short int getType(){ return 0; } + 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; } 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); - - 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); + 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); - } 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(); } |