From b64d5809e7e3b832e2a011869db68e70b4b4e6fc Mon Sep 17 00:00:00 2001 From: Brecht Van Lommel Date: Sun, 17 Jan 2016 21:35:32 +0100 Subject: Upgrade Bullet to version 2.83. I tried to carefully preserve all patches since the last upgrade. Improves T47195, cloth collision detection bug. Differential Revision: https://developer.blender.org/D1739 --- .../BulletDynamics/MLCPSolvers/btDantzigLCP.cpp | 17 +- .../MLCPSolvers/btLemkeAlgorithm.cpp | 371 +++++++++++++++++++++ .../BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h | 108 ++++++ .../src/BulletDynamics/MLCPSolvers/btLemkeSolver.h | 350 +++++++++++++++++++ .../BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 160 ++++----- .../src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 4 +- .../MLCPSolvers/btSolveProjectedGaussSeidel.h | 2 +- 7 files changed, 926 insertions(+), 86 deletions(-) create mode 100644 extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp create mode 100644 extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h create mode 100644 extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h (limited to 'extern/bullet2/src/BulletDynamics/MLCPSolvers') diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp index 3bf7b5c1311..986f2148701 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp @@ -821,14 +821,15 @@ void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) /* declare variables - Z matrix, p and q vectors, etc */ btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; const btScalar *ell; - int lskip2,lskip3,i,j; + int lskip2,i,j; +// int lskip3; /* special handling for L and B because we're solving L1 *transpose* */ L = L + (n-1)*(lskip1+1); B = B + n-1; lskip1 = -lskip1; /* compute lskip values */ lskip2 = 2*lskip1; - lskip3 = 3*lskip1; + //lskip3 = 3*lskip1; /* compute all 4 x 1 blocks of X */ for (i=0; i <= n-4; i+=4) { /* compute all 4 x 1 block of X, from rows i..i+4-1 */ @@ -1199,9 +1200,9 @@ struct btLCP int *const m_findex, *const m_p, *const m_C; btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows); + bool *_state, int *_findex, int *p, int *c, btScalar **Arows); int getNub() const { return m_nub; } void transfer_i_to_C (int i); void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1 @@ -1224,9 +1225,9 @@ struct btLCP btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, - btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, + btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d, btScalar *_Dell, btScalar *_ell, btScalar *_tmp, - bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows): + bool *_state, int *_findex, int *p, int *c, btScalar **Arows): m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), # ifdef BTROWPTRS m_A(Arows), @@ -1234,8 +1235,8 @@ btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btSc m_A(_Adata), #endif m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi), - m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), - m_state(_state), m_findex(_findex), m_p(_p), m_C(_C) + m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), + m_state(_state), m_findex(_findex), m_p(p), m_C(c) { { btSetZero (m_x,m_n); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp new file mode 100644 index 00000000000..1f4015c7c72 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp @@ -0,0 +1,371 @@ +/* Copyright (C) 2004-2013 MBSim Development Team + +Code was converted for the Bullet Continuous Collision Detection and Physics Library + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//The original version is here +//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc +//This file is re-distributed under the ZLib license, with permission of the original author +//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h +//STL/std::vector replaced by btAlignedObjectArray + + + +#include "btLemkeAlgorithm.h" + +#undef BT_DEBUG_OSTREAM +#ifdef BT_DEBUG_OSTREAM +using namespace std; +#endif //BT_DEBUG_OSTREAM + +btScalar btMachEps() +{ + static bool calculated=false; + static btScalar machEps = btScalar(1.); + if (!calculated) + { + do { + machEps /= btScalar(2.0); + // If next epsilon yields 1, then break, because current + // epsilon is the machine epsilon. + } + while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0)); +// printf( "\nCalculated Machine epsilon: %G\n", machEps ); + calculated=true; + } + return machEps; +} + +btScalar btEpsRoot() { + + static btScalar epsroot = 0.; + static bool alreadyCalculated = false; + + if (!alreadyCalculated) { + epsroot = btSqrt(btMachEps()); + alreadyCalculated = true; + } + return epsroot; +} + + + + btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/) +{ + + + steps = 0; + + int dim = m_q.size(); +#ifdef BT_DEBUG_OSTREAM + if(DEBUGLEVEL >= 1) { + cout << "Dimension = " << dim << endl; + } +#endif //BT_DEBUG_OSTREAM + + btVectorXu solutionVector(2 * dim); + solutionVector.setZero(); + + //, INIT, 0.); + + btMatrixXu ident(dim, dim); + ident.setIdentity(); +#ifdef BT_DEBUG_OSTREAM + cout << m_M << std::endl; +#endif + + btMatrixXu mNeg = m_M.negative(); + + btMatrixXu A(dim, 2 * dim + 2); + // + A.setSubMatrix(0, 0, dim - 1, dim - 1,ident); + A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg); + A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f); + A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q); + +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //BT_DEBUG_OSTREAM + + + // btVectorXu q_; + // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1); + + btAlignedObjectArray basis; + //At first, all w-values are in the basis + for (int i = 0; i < dim; i++) + basis.push_back(i); + + int pivotRowIndex = -1; + btScalar minValue = 1e30f; + bool greaterZero = true; + for (int i=0;i= 3) + { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM + + if (!greaterZero) + { + + if (maxloops == 0) { + maxloops = 100; +// maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<= 3) { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + cout << "Basis: "; + for (int i = 0; i < basis.size(); i++) + cout << basis[i] << " "; + cout << endl; + } +#endif //BT_DEBUG_OSTREAM + + int pivotColIndexOld = pivotColIndex; + + /*find new column index */ + if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value + pivotColIndex = basis[pivotRowIndex] + dim; + else + //else do it the other way round and get in the corresponding w-value + pivotColIndex = basis[pivotRowIndex] - dim; + + /*the column becomes part of the basis*/ + basis[pivotRowIndex] = pivotColIndexOld; + + pivotRowIndex = findLexicographicMinimum(A, pivotColIndex); + + if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary + GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis); + basis[pivotRowIndex] = pivotColIndex; //update basis + break; + } + + } +#ifdef BT_DEBUG_OSTREAM + if(DEBUGLEVEL >= 1) { + cout << "Number of loops: " << steps << endl; + cout << "Number of maximal loops: " << maxloops << endl; + } +#endif //BT_DEBUG_OSTREAM + + if(!validBasis(basis)) { + info = -1; +#ifdef BT_DEBUG_OSTREAM + if(DEBUGLEVEL >= 1) + cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl; +#endif //BT_DEBUG_OSTREAM + + return solutionVector; + } + + } +#ifdef BT_DEBUG_OSTREAM + if (DEBUGLEVEL >= 2) { + // cout << "A: " << A << endl; + cout << "pivotRowIndex " << pivotRowIndex << endl; + cout << "pivotColIndex " << pivotColIndex << endl; + } +#endif //BT_DEBUG_OSTREAM + + for (int i = 0; i < basis.size(); i++) + { + solutionVector[basis[i]] = A(i,2*dim+1);//q_[i]; + } + + info = 0; + + return solutionVector; + } + + int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) { + int RowIndex = 0; + int dim = A.rows(); + btAlignedObjectArray Rows; + for (int row = 0; row < dim; row++) + { + + btVectorXu vec(dim + 1); + vec.setZero();//, INIT, 0.) + Rows.push_back(vec); + btScalar a = A(row, pivotColIndex); + if (a > 0) { + Rows[row][0] = A(row, 2 * dim + 1) / a; + Rows[row][1] = A(row, 2 * dim) / a; + for (int j = 2; j < dim + 1; j++) + Rows[row][j] = A(row, j - 1) / a; + +#ifdef BT_DEBUG_OSTREAM + // if (DEBUGLEVEL) { + // cout << "Rows(" << row << ") = " << Rows[row] << endl; + // } +#endif + } + } + + for (int i = 0; i < Rows.size(); i++) + { + if (Rows[i].nrm2() > 0.) { + + int j = 0; + for (; j < Rows.size(); j++) + { + if(i != j) + { + if(Rows[j].nrm2() > 0.) + { + btVectorXu test(dim + 1); + for (int ii=0;ii 0) + return true; + + return false; + } + +void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis) +{ + + btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex); +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif + + for (int i = 0; i < A.rows(); i++) + { + if (i != pivotRowIndex) + { + for (int j = 0; j < A.cols(); j++) + { + if (j != pivotColumnIndex) + { + btScalar v = A(i, j); + v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a; + A.setElem(i, j, v); + } + } + } + } + +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //BT_DEBUG_OSTREAM + for (int i = 0; i < A.cols(); i++) + { + A.mulElem(pivotRowIndex, i,-a); + } +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //#ifdef BT_DEBUG_OSTREAM + + for (int i = 0; i < A.rows(); i++) + { + if (i != pivotRowIndex) + { + A.setElem(i, pivotColumnIndex,0); + } + } +#ifdef BT_DEBUG_OSTREAM + cout << A << std::endl; +#endif //#ifdef BT_DEBUG_OSTREAM + } + + bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector) +{ + bool isGreater = true; + for (int i = 0; i < vector.size(); i++) { + if (vector[i] < 0) { + isGreater = false; + break; + } + } + + return isGreater; + } + + bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray& basis) + { + bool isValid = true; + for (int i = 0; i < basis.size(); i++) { + if (basis[i] >= basis.size() * 2) { //then z0 is in the base + isValid = false; + break; + } + } + + return isValid; + } + + diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h new file mode 100644 index 00000000000..7555cd9d207 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h @@ -0,0 +1,108 @@ +/* Copyright (C) 2004-2013 MBSim Development Team + +Code was converted for the Bullet Continuous Collision Detection and Physics Library + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +//The original version is here +//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc +//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl) +//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h +//STL/std::vector replaced by btAlignedObjectArray + + + +#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_ +#define BT_NUMERICS_LEMKE_ALGORITHM_H_ + +#include "LinearMath/btMatrixX.h" + + +#include //todo: replace by btAlignedObjectArray + +class btLemkeAlgorithm +{ +public: + + + btLemkeAlgorithm(const btMatrixXu& M_, const btVectorXu& q_, const int & DEBUGLEVEL_ = 0) : + DEBUGLEVEL(DEBUGLEVEL_) + { + setSystem(M_, q_); + } + + /* GETTER / SETTER */ + /** + * \brief return info of solution process + */ + int getInfo() { + return info; + } + + /** + * \brief get the number of steps until the solution was found + */ + int getSteps(void) { + return steps; + } + + + + /** + * \brief set system with Matrix M and vector q + */ + void setSystem(const btMatrixXu & M_, const btVectorXu & q_) + { + m_M = M_; + m_q = q_; + } + /***************************************************/ + + /** + * \brief solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) + */ + btVectorXu solve(unsigned int maxloops = 0); + + virtual ~btLemkeAlgorithm() { + } + +protected: + int findLexicographicMinimum(const btMatrixXu &A, const int & pivotColIndex); + bool LexicographicPositive(const btVectorXu & v); + void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray& basis); + bool greaterZero(const btVectorXu & vector); + bool validBasis(const btAlignedObjectArray& basis); + + btMatrixXu m_M; + btVectorXu m_q; + + /** + * \brief number of steps until the Lemke algorithm found a solution + */ + unsigned int steps; + + /** + * \brief define level of debug output + */ + int DEBUGLEVEL; + + /** + * \brief did the algorithm find a solution + * + * -1 : not successful + * 0 : successful + */ + int info; +}; + + +#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */ diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h new file mode 100644 index 00000000000..98484c37964 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h @@ -0,0 +1,350 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_LEMKE_SOLVER_H +#define BT_LEMKE_SOLVER_H + + +#include "btMLCPSolverInterface.h" +#include "btLemkeAlgorithm.h" + + + + +///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) " +///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time. +///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team +class btLemkeSolver : public btMLCPSolverInterface +{ +protected: + +public: + + btScalar m_maxValue; + int m_debugLevel; + int m_maxLoops; + bool m_useLoHighBounds; + + + + btLemkeSolver() + :m_maxValue(100000), + m_debugLevel(0), + m_maxLoops(1000), + m_useLoHighBounds(true) + { + } + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray& limitDependency, int numIterations, bool useSparsity = true) + { + + if (m_useLoHighBounds) + { + + BT_PROFILE("btLemkeSolver::solveMLCP"); + int n = A.rows(); + if (0==n) + return true; + + bool fail = false; + + btVectorXu solution(n); + btVectorXu q1; + q1.resize(n); + for (int row=0;rowm_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]m_maxValue) + { + if (x[i]> errorValueMax) + { + fail = true; + errorIndexMax = i; + errorValueMax = x[i]; + } + ////printf("x[i] = %f,",x[i]); + } + if (x[i]<-m_maxValue) + { + if (x[i]m_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { - btScalar rhs = m_allConstraintArray[i].m_rhs; - btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration; + btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; + btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; m_b[i]=rhs/jacDiag; m_bSplit[i] = rhsPenetration/jacDiag; } @@ -178,8 +178,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } } - btScalar* w = 0; - int nub = 0; +// btScalar* w = 0; +// int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); @@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_hi[i] = BT_INFINITY; } else { - m_lo[i] = m_allConstraintArray[i].m_lowerLimit; - m_hi[i] = m_allConstraintArray[i].m_upperLimit; + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; } } } // - int m=m_allConstraintArray.size(); + int m=m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray bodyJointNodeArray; @@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btAlignedObjectArray jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintArray.size()); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); } static btMatrixXu J3; @@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { BT_PROFILE("ofs resize"); ofs.resize(0); - ofs.resizeNoInitialize(m_allConstraintArray.size()); + ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); } { BT_PROFILE("Compute J and JinvM"); @@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) int numRows = 0; - for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } for (int row=0;rowgetInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMass[r]); JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); } @@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) for (int row=0;rowgetInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); for (int r=0;r<3;r++) { - J3.setElem(cur,r,m_allConstraintArray[i+row].m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[r]); + J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); + J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); JinvM3.setElem(cur,r,normalInvMassB[r]); JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); } @@ -349,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;im_solverBodyIdA; + int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; numRows = im_solverBodyIdB == sbA) ? 8*numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); m_A.multiplyAdd2_p8r ( JinvMrow, Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); @@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (j1m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); } @@ -404,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) // compute diagonal blocks of m_A int row__ = 0; - int numJointRows = m_allConstraintArray.size(); + int numJointRows = m_allConstraintPtrArray.size(); int jj=0; for (;row__m_solverBodyIdA; + int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; @@ -453,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) { - for (int i=0;im_tmpSolverBodyPool.size(); - int numConstraintRows = m_allConstraintArray.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) @@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;im_jacDiagABInv) { - m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; + m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; } } @@ -517,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) for (int i=0;im_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; - int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; + int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); + setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); + setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } @@ -573,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_xSplit.resize(numConstraintRows); // m_x.setZero(); - for (int i=0;i m_limitDependencies; - btConstraintArray m_allConstraintArray; + btAlignedObjectArray m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; btScalar m_cfm; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); + + virtual void createMLCP(const btContactSolverInfo& infoGlobal); virtual void createMLCPFast(const btContactSolverInfo& infoGlobal); diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index d2ad54d21a8..77cc57c6e0e 100644 --- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -62,7 +62,7 @@ public: } float aDiag = A(i,i); - x [i] = (b [i] - delta) / A(i,i); + x [i] = (b [i] - delta) / aDiag; float s = 1.f; if (limitDependency[i]>=0) -- cgit v1.2.3