diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 373cdfa26..97674a4e5 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -660,26 +660,25 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int totalNumRows = 0; int i; + + m_tmpConstraintSizesPool.resize(numConstraints); //calculate the total number of contraint rows for (i=0;igetInfo1(&info1); totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resize(totalNumRows); - btTypedConstraint::btConstraintInfo1 info1; - info1.m_numConstraintRows = 0; - - + ///setup the btSolverConstraints int currentRow = 0; - for (i=0;igetInfo1(&info1); + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + if (info1.m_numConstraintRows) { btAssert(currentRowneedsFeedback()) + { + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + if (info1.m_numConstraintRows) + { + btAssert(currentRowgetAppliedLinearImpulse().setValue(0,0,0); + constraint->getAppliedAngularImpulseA().setValue(0,0,0); + constraint->getAppliedAngularImpulseB().setValue(0,0,0); + int j; + btScalar maxAppliedImpulse(0.); + for ( j=0;jgetAppliedLinearImpulse() += currentConstraintRow[i].m_contactNormal*currentConstraintRow[i].m_appliedImpulse; + constraint->getAppliedAngularImpulseA() += currentConstraintRow[i].m_relpos1CrossNormal*currentConstraintRow[i].m_appliedImpulse; + constraint->getAppliedAngularImpulseB() += currentConstraintRow[i].m_relpos2CrossNormal*currentConstraintRow[i].m_appliedImpulse; + } + constraint->internalSetAppliedImpulse(maxAppliedImpulse); + } + } + currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows; + } +#endif if (infoGlobal.m_splitImpulse) { for ( i=0;i m_orderTmpConstraintPool; btAlignedObjectArray m_orderFrictionConstraintPool; + btAlignedObjectArray m_tmpConstraintSizesPool; 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); diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index ed155ba30..996e93fca 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -24,6 +24,7 @@ static btRigidBody s_fixed(0, 0,0); btTypedConstraint::btTypedConstraint(btTypedConstraintType type) :m_userConstraintType(-1), m_userConstraintId(-1), +m_needsFeedback(false), m_constraintType (type), m_rbA(s_fixed), m_rbB(s_fixed), @@ -35,6 +36,7 @@ m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) :m_userConstraintType(-1), m_userConstraintId(-1), +m_needsFeedback(false), m_constraintType (type), m_rbA(rbA), m_rbB(s_fixed), @@ -49,6 +51,7 @@ m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) :m_userConstraintType(-1), m_userConstraintId(-1), +m_needsFeedback(false), m_constraintType (type), m_rbA(rbA), m_rbB(rbB), diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 14cbe831b..c2e5ad862 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -38,6 +38,7 @@ class btTypedConstraint { int m_userConstraintType; int m_userConstraintId; + bool m_needsFeedback; btTypedConstraintType m_constraintType; @@ -54,6 +55,9 @@ protected: btScalar m_appliedImpulse; btScalar m_dbgDrawSize; + btVector3 m_appliedLinearImpulse; + btVector3 m_appliedAngularImpulseA; + btVector3 m_appliedAngularImpulseB; public: @@ -95,18 +99,30 @@ public: int *findex; }; - + ///internal method used by the constraint solver, don't use them directly virtual void buildJacobian() = 0; + ///internal method used by the constraint solver, don't use them directly virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) { } + + ///internal method used by the constraint solver, don't use them directly virtual void getInfo1 (btConstraintInfo1* info)=0; + ///internal method used by the constraint solver, don't use them directly virtual void getInfo2 (btConstraintInfo2* info)=0; + ///internal method used by the constraint solver, don't use them directly + void internalSetAppliedImpulse(btScalar appliedImpulse) + { + m_appliedImpulse = appliedImpulse; + } + + ///internal method used by the constraint solver, don't use them directly virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + ///internal method used by the constraint solver, don't use them directly btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); const btRigidBody& getRigidBodyA() const @@ -152,11 +168,64 @@ public: return m_userConstraintId; } + bool needsFeedback() const + { + return m_needsFeedback; + } + + ///enableFeedback will allow to read the applied linear and angular impulse + ///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information + void enableFeedback(bool needsFeedback) + { + m_needsFeedback = needsFeedback; + } + + ///getAppliedImpulse is an estimated total applied impulse. + ///This feedback could be used to determine breaking constraints or playing sounds. btScalar getAppliedImpulse() const { + btAssert(m_needsFeedback); return m_appliedImpulse; } + const btVector3& getAppliedLinearImpulse() const + { + btAssert(m_needsFeedback); + return m_appliedLinearImpulse; + } + + btVector3& getAppliedLinearImpulse() + { + btAssert(m_needsFeedback); + return m_appliedLinearImpulse; + } + + const btVector3& getAppliedAngularImpulseA() const + { + btAssert(m_needsFeedback); + return m_appliedAngularImpulseA; + } + + btVector3& getAppliedAngularImpulseA() + { + btAssert(m_needsFeedback); + return m_appliedAngularImpulseA; + } + + const btVector3& getAppliedAngularImpulseB() const + { + btAssert(m_needsFeedback); + return m_appliedAngularImpulseB; + } + + btVector3& getAppliedAngularImpulseB() + { + btAssert(m_needsFeedback); + return m_appliedAngularImpulseB; + } + + + btTypedConstraintType getConstraintType () const { return m_constraintType; diff --git a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp index 3a242244f..ee19c3961 100644 --- a/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp +++ b/src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp @@ -1876,7 +1876,7 @@ void processSolverTask(void* userPtr, void* lsMemory) // DMA the constraints { - int dmaSize = sizeof(btSolverConstraint)*packetSize; + int dmaSize = sizeof(btSolverConstraint)*(int)packetSize; uint64_t dmaPpuAddress2 = reinterpret_cast (taskDesc.m_solverData.m_solverConstraintList + constraintListOffset); cellDmaLargeGet(constraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); } @@ -1895,7 +1895,7 @@ void processSolverTask(void* userPtr, void* lsMemory) // Write back the constraints for accumulated stuff { - int dmaSize = sizeof(btSolverConstraint)*packetSize; + int dmaSize = sizeof(btSolverConstraint)*(int)packetSize; uint64_t dmaPpuAddress2 = reinterpret_cast (taskDesc.m_solverData.m_solverConstraintList + constraintListOffset); cellDmaLargePut(constraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); } @@ -1923,7 +1923,7 @@ void processSolverTask(void* userPtr, void* lsMemory) // DMA the constraints { - int dmaSize = sizeof(btSolverConstraint)*packetSize*3; + int dmaSize = sizeof(btSolverConstraint)*(int)packetSize*3; uint64_t dmaPpuAddress2 = reinterpret_cast (taskDesc.m_solverData.m_solverInternalConstraintList + constraintListOffset); cellDmaLargeGet(internalConstraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); } @@ -1966,7 +1966,7 @@ void processSolverTask(void* userPtr, void* lsMemory) // Write back the constraints for accumulated stuff { - int dmaSize = sizeof(btSolverConstraint)*packetSize*3; + int dmaSize = sizeof(btSolverConstraint)*(int)packetSize*3; uint64_t dmaPpuAddress2 = reinterpret_cast (taskDesc.m_solverData.m_solverInternalConstraintList + constraintListOffset); cellDmaLargePut(internalConstraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); } @@ -2107,7 +2107,7 @@ void processSolverTask(void* userPtr, void* lsMemory) // DMA the constraints { - int dmaSize = sizeof(btSolverConstraint)*packetSize*3; + int dmaSize = sizeof(btSolverConstraint)*(int)packetSize*3; uint64_t dmaPpuAddress2 = reinterpret_cast (taskDesc.m_solverData.m_solverInternalConstraintList + constraintListOffset); cellDmaLargeGet(internalConstraints, dmaPpuAddress2, dmaSize, DMA_TAG(1), 0, 0); }