Added a Lemke MLCP solver, extracted from the MBSim project, and re-licensed under the zlib license
with permission of the original author. The Lemke implementation is not fully working yet: 1) we need to convert the lo-high LCP problem into a problem without the lo/high 2) we need to sort out the remaining instabilities, and report a failure if the max loopcount is reached etc. We replaced the fmatvec library with our own LinearMath/btMatrixX.h, and STL std::vector with btAlignedObjectArray Removed some warnings/potential issues: use fuzzyZero instead of isZero, and some warnings, related to this issue 756
This commit is contained in:
erwin.coumans
@@ -812,7 +812,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
|
||||
|
||||
|
||||
///avoid collision response between two static objects
|
||||
if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
|
||||
if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
|
||||
return;
|
||||
|
||||
int rollingFriction=1;
|
||||
|
||||
368
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
Normal file
368
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
Normal file
@@ -0,0 +1,368 @@
|
||||
/* 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"
|
||||
|
||||
|
||||
|
||||
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<int> 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<dim;i++)
|
||||
{
|
||||
btScalar v =A(i,2*dim+1);
|
||||
if (v<minValue)
|
||||
{
|
||||
minValue=v;
|
||||
pivotRowIndex = i;
|
||||
}
|
||||
if (v<0)
|
||||
greaterZero = false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
|
||||
int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
|
||||
int pivotColIndex = 2 * dim; // first col is that of z0
|
||||
|
||||
#ifdef BT_DEBUG_OSTREAM
|
||||
if (DEBUGLEVEL >= 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<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
|
||||
}
|
||||
|
||||
/*start looping*/
|
||||
for(steps = 0; steps < maxloops; steps++) {
|
||||
|
||||
GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
|
||||
#ifdef BT_DEBUG_OSTREAM
|
||||
if (DEBUGLEVEL >= 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<btVectorXu> 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<dim+1;ii++)
|
||||
{
|
||||
test[ii] = Rows[j][ii] - Rows[i][ii];
|
||||
}
|
||||
|
||||
//=Rows[j] - Rows[i]
|
||||
if (! LexicographicPositive(test))
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (j == Rows.size())
|
||||
{
|
||||
RowIndex += i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return RowIndex;
|
||||
}
|
||||
|
||||
bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
|
||||
{
|
||||
int i = 0;
|
||||
// if (DEBUGLEVEL)
|
||||
// cout << "v " << v << endl;
|
||||
|
||||
while(i < v.size()-1 && fabs(v[i]) < btMachEps())
|
||||
i++;
|
||||
if (v[i] > 0)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& 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<int>& 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;
|
||||
}
|
||||
|
||||
|
||||
108
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
Normal file
108
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
Normal file
@@ -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
|
||||
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
|
||||
//STL/std::vector replaced by btAlignedObjectArray
|
||||
|
||||
|
||||
|
||||
#ifndef NUMERICS_LEMKE_ALGORITHM_H_
|
||||
#define NUMERICS_LEMKE_ALGORITHM_H_
|
||||
|
||||
#include "LinearMath/btMatrixX.h"
|
||||
|
||||
|
||||
#include <vector> //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<int>& basis);
|
||||
bool greaterZero(const btVectorXu & vector);
|
||||
bool validBasis(const btAlignedObjectArray<int>& 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 /* NUMERICS_LEMKE_ALGORITHM_H_ */
|
||||
127
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
Normal file
127
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
Normal file
@@ -0,0 +1,127 @@
|
||||
/*
|
||||
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"
|
||||
|
||||
|
||||
class btLemkeSolver : public btMLCPSolverInterface
|
||||
{
|
||||
public:
|
||||
virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
|
||||
{
|
||||
int dimension = A.rows();
|
||||
if (0==dimension)
|
||||
return true;
|
||||
|
||||
// printf("================ solving using Lemke/Newton/Fixpoint\n");
|
||||
|
||||
btVectorXu q;
|
||||
q.resize(dimension);
|
||||
for (int row=0;row<dimension;row++)
|
||||
{
|
||||
q[row] = -b[row];
|
||||
}
|
||||
|
||||
int debugLevel=0;
|
||||
btLemkeAlgorithm lemke(A,q,debugLevel);
|
||||
|
||||
|
||||
lemke.setSystem(A,q);
|
||||
|
||||
int maxloops = 10000;
|
||||
btVectorXu solution = lemke.solve(maxloops);
|
||||
|
||||
//check solution
|
||||
|
||||
bool fail = false;
|
||||
int errorIndexMax = -1;
|
||||
int errorIndexMin = -1;
|
||||
float errorValueMax = -1e30;
|
||||
float errorValueMin = 1e30;
|
||||
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i] = solution[i+dimension];
|
||||
volatile btScalar check = x[i];
|
||||
if (x[i] != check)
|
||||
{
|
||||
x.setZero();
|
||||
return false;
|
||||
}
|
||||
|
||||
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
|
||||
//we need to figure out why it happens, and fix it, or detect it properly)
|
||||
if (x[i]>100000)
|
||||
{
|
||||
if (x[i]> errorValueMax)
|
||||
{
|
||||
fail = true;
|
||||
errorIndexMax = i;
|
||||
errorValueMax = x[i];
|
||||
}
|
||||
////printf("x[i] = %f,",x[i]);
|
||||
}
|
||||
if (x[i]<-10000)
|
||||
{
|
||||
if (x[i]<errorValueMin)
|
||||
{
|
||||
errorIndexMin = i;
|
||||
errorValueMin = x[i];
|
||||
fail = true;
|
||||
//printf("x[i] = %f,",x[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (fail)
|
||||
{
|
||||
static int errorCountTimes = 0;
|
||||
if (errorIndexMin<0)
|
||||
errorValueMin = 0.f;
|
||||
if (errorIndexMax<0)
|
||||
errorValueMax = 0.f;
|
||||
printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i]=0.f;
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
if (lemke.getInfo()<0)
|
||||
{
|
||||
printf("Lemke found no solution, info = %d\n",lemke.getInfo());
|
||||
} else
|
||||
{
|
||||
printf("Lemke info = %d, found a solution in %d steps\n",lemke.getInfo(),lemke.getSteps());
|
||||
//printf("Lemke found a solution\n");
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i] = solution(i+dimension);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
return !fail;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_LEMKE_SOLVER_H
|
||||
@@ -20,6 +20,12 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
//#define BT_DEBUG_OSTREAM
|
||||
#ifdef BT_DEBUG_OSTREAM
|
||||
#include <ostream>
|
||||
#include <iomanip> // std::setw
|
||||
#endif //BT_DEBUG_OSTREAM
|
||||
|
||||
class btIntSortPredicate
|
||||
{
|
||||
public:
|
||||
@@ -30,6 +36,118 @@ class btIntSortPredicate
|
||||
};
|
||||
|
||||
|
||||
template <typename T>
|
||||
struct btVectorX
|
||||
{
|
||||
btAlignedObjectArray<T> m_storage;
|
||||
|
||||
btVectorX()
|
||||
{
|
||||
}
|
||||
btVectorX(int numRows)
|
||||
{
|
||||
m_storage.resize(numRows);
|
||||
}
|
||||
|
||||
void resize(int rows)
|
||||
{
|
||||
m_storage.resize(rows);
|
||||
}
|
||||
int cols() const
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
int rows() const
|
||||
{
|
||||
return m_storage.size();
|
||||
}
|
||||
int size() const
|
||||
{
|
||||
return rows();
|
||||
}
|
||||
|
||||
T nrm2() const
|
||||
{
|
||||
T norm = T(0);
|
||||
|
||||
int nn = rows();
|
||||
|
||||
{
|
||||
if (nn == 1)
|
||||
{
|
||||
norm = btFabs((*this)[0]);
|
||||
}
|
||||
else
|
||||
{
|
||||
T scale = 0.0;
|
||||
T ssq = 1.0;
|
||||
|
||||
/* The following loop is equivalent to this call to the LAPACK
|
||||
auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
|
||||
|
||||
for (int ix=0;ix<nn;ix++)
|
||||
{
|
||||
if ((*this)[ix] != 0.0)
|
||||
{
|
||||
T absxi = btFabs((*this)[ix]);
|
||||
if (scale < absxi)
|
||||
{
|
||||
T temp;
|
||||
temp = scale / absxi;
|
||||
ssq = ssq * (temp * temp) + 1.0;
|
||||
scale = absxi;
|
||||
}
|
||||
else
|
||||
{
|
||||
T temp;
|
||||
temp = absxi / scale;
|
||||
ssq += temp * temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
norm = scale * sqrt(ssq);
|
||||
}
|
||||
}
|
||||
return norm;
|
||||
|
||||
}
|
||||
void setZero()
|
||||
{
|
||||
// for (int i=0;i<m_storage.size();i++)
|
||||
// m_storage[i]=0;
|
||||
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
|
||||
btSetZero(&m_storage[0],m_storage.size());
|
||||
}
|
||||
const T& operator[] (int index) const
|
||||
{
|
||||
return m_storage[index];
|
||||
}
|
||||
|
||||
T& operator[] (int index)
|
||||
{
|
||||
return m_storage[index];
|
||||
}
|
||||
|
||||
T* getBufferPointerWritable()
|
||||
{
|
||||
return m_storage.size() ? &m_storage[0] : 0;
|
||||
}
|
||||
|
||||
const T* getBufferPointer() const
|
||||
{
|
||||
return m_storage.size() ? &m_storage[0] : 0;
|
||||
}
|
||||
|
||||
};
|
||||
/*
|
||||
template <typename T>
|
||||
void setElem(btMatrixX<T>& mat, int row, int col, T val)
|
||||
{
|
||||
mat.setElem(row,col,val);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
template <typename T>
|
||||
struct btMatrixX
|
||||
{
|
||||
@@ -109,21 +227,7 @@ struct btMatrixX
|
||||
}
|
||||
}
|
||||
|
||||
void copyLowerToUpperTriangle()
|
||||
{
|
||||
int count=0;
|
||||
for (int row=0;row<m_rowNonZeroElements1.size();row++)
|
||||
{
|
||||
for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
|
||||
{
|
||||
int col = m_rowNonZeroElements1[row][j];
|
||||
setElem(col,row, (*this)(row,col));
|
||||
count++;
|
||||
|
||||
}
|
||||
}
|
||||
//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
|
||||
}
|
||||
|
||||
void setElem(int row,int col, T val)
|
||||
{
|
||||
m_setElemOperations++;
|
||||
@@ -134,9 +238,37 @@ struct btMatrixX
|
||||
m_rowNonZeroElements1[row].push_back(col);
|
||||
m_colNonZeroElements[col].push_back(row);
|
||||
}
|
||||
m_storage[row*m_cols+col] = val;
|
||||
}
|
||||
m_storage[row*m_cols+col] = val;
|
||||
}
|
||||
|
||||
void mulElem(int row,int col, T val)
|
||||
{
|
||||
m_setElemOperations++;
|
||||
//mul doesn't change sparsity info
|
||||
|
||||
m_storage[row*m_cols+col] *= val;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void copyLowerToUpperTriangle()
|
||||
{
|
||||
int count=0;
|
||||
for (int row=0;row<m_rowNonZeroElements1.size();row++)
|
||||
{
|
||||
for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
|
||||
{
|
||||
int col = m_rowNonZeroElements1[row][j];
|
||||
setElem(col,row, (*this)(row,col));
|
||||
count++;
|
||||
|
||||
}
|
||||
}
|
||||
//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
|
||||
}
|
||||
|
||||
const T& operator() (int row,int col) const
|
||||
{
|
||||
return m_storage[col+row*m_cols];
|
||||
@@ -167,6 +299,19 @@ struct btMatrixX
|
||||
clearSparseInfo();
|
||||
}
|
||||
}
|
||||
|
||||
void setIdentity()
|
||||
{
|
||||
btAssert(rows() == cols());
|
||||
|
||||
setZero();
|
||||
for (int row=0;row<rows();row++)
|
||||
{
|
||||
setElem(row,row,1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void printMatrix(const char* msg)
|
||||
{
|
||||
@@ -404,73 +549,62 @@ struct btMatrixX
|
||||
bb += 8;
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct btVectorX
|
||||
{
|
||||
btAlignedObjectArray<T> m_storage;
|
||||
|
||||
btVectorX()
|
||||
|
||||
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
|
||||
{
|
||||
int numRows = rowend+1-rowstart;
|
||||
int numCols = colend+1-colstart;
|
||||
|
||||
for (int row=0;row<numRows;row++)
|
||||
{
|
||||
for (int col=0;col<numCols;col++)
|
||||
{
|
||||
setElem(rowstart+row,colstart+col,value);
|
||||
}
|
||||
}
|
||||
}
|
||||
btVectorX(int numRows)
|
||||
|
||||
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
|
||||
{
|
||||
m_storage.resize(numRows);
|
||||
btAssert(rowend+1-rowstart == block.rows());
|
||||
btAssert(colend+1-colstart == block.cols());
|
||||
for (int row=0;row<block.rows();row++)
|
||||
{
|
||||
for (int col=0;col<block.cols();col++)
|
||||
{
|
||||
setElem(rowstart+row,colstart+col,block(row,col));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void resize(int rows)
|
||||
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
|
||||
{
|
||||
m_storage.resize(rows);
|
||||
btAssert(rowend+1-rowstart == block.rows());
|
||||
btAssert(colend+1-colstart == block.cols());
|
||||
for (int row=0;row<block.rows();row++)
|
||||
{
|
||||
for (int col=0;col<block.cols();col++)
|
||||
{
|
||||
setElem(rowstart+row,colstart+col,block[row]);
|
||||
}
|
||||
}
|
||||
}
|
||||
int cols() const
|
||||
|
||||
|
||||
btMatrixX negative()
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
int rows() const
|
||||
{
|
||||
return m_storage.size();
|
||||
}
|
||||
int size() const
|
||||
{
|
||||
return rows();
|
||||
}
|
||||
void setZero()
|
||||
{
|
||||
// for (int i=0;i<m_storage.size();i++)
|
||||
// m_storage[i]=0;
|
||||
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
|
||||
btSetZero(&m_storage[0],m_storage.size());
|
||||
}
|
||||
const T& operator[] (int index) const
|
||||
{
|
||||
return m_storage[index];
|
||||
}
|
||||
|
||||
T& operator[] (int index)
|
||||
{
|
||||
return m_storage[index];
|
||||
}
|
||||
|
||||
T* getBufferPointerWritable()
|
||||
{
|
||||
return m_storage.size() ? &m_storage[0] : 0;
|
||||
}
|
||||
|
||||
const T* getBufferPointer() const
|
||||
{
|
||||
return m_storage.size() ? &m_storage[0] : 0;
|
||||
btMatrixX neg(rows(),cols());
|
||||
for (int i=0;i<m_colNonZeroElements.size();i++)
|
||||
for (int h=0;h<m_colNonZeroElements[i].size();h++)
|
||||
{
|
||||
int j = m_colNonZeroElements[i][h];
|
||||
T v = (*this)(i,j);
|
||||
neg.setElem(i,j,-v);
|
||||
}
|
||||
return neg;
|
||||
}
|
||||
|
||||
};
|
||||
/*
|
||||
template <typename T>
|
||||
void setElem(btMatrixX<T>& mat, int row, int col, T val)
|
||||
{
|
||||
mat.setElem(row,col,val);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
typedef btMatrixX<float> btMatrixXf;
|
||||
@@ -480,6 +614,30 @@ typedef btMatrixX<double> btMatrixXd;
|
||||
typedef btVectorX<double> btVectorXd;
|
||||
|
||||
|
||||
#ifdef BT_DEBUG_OSTREAM
|
||||
template <typename T>
|
||||
std::ostream& operator<< (std::ostream& os, btMatrixX<T>& mat)
|
||||
{
|
||||
|
||||
os << " [";
|
||||
//printf("%s ---------------------\n",msg);
|
||||
for (int i=0;i<mat.rows();i++)
|
||||
{
|
||||
for (int j=0;j<mat.cols();j++)
|
||||
{
|
||||
os << std::setw(10) << mat(i,j);
|
||||
}
|
||||
if (i!=mat.rows()-1)
|
||||
os << std::endl << " ";
|
||||
}
|
||||
os << " ]";
|
||||
//printf("\n---------------------\n");
|
||||
|
||||
return os;
|
||||
}
|
||||
|
||||
#endif //BT_DEBUG_OSTREAM
|
||||
|
||||
|
||||
inline void setElem(btMatrixXd& mat, int row, int col, double val)
|
||||
{
|
||||
|
||||
@@ -297,7 +297,7 @@ public:
|
||||
SIMD_FORCE_INLINE btVector3& normalize()
|
||||
{
|
||||
|
||||
btAssert(length() != btScalar(0));
|
||||
btAssert(!fuzzyZero());
|
||||
|
||||
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
|
||||
// dot product first
|
||||
@@ -685,6 +685,7 @@ public:
|
||||
return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE bool fuzzyZero() const
|
||||
{
|
||||
return length2() < SIMD_EPSILON;
|
||||
@@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
|
||||
{
|
||||
btVector3 norm = *this;
|
||||
btVector3 nrm = *this;
|
||||
|
||||
return norm.normalize();
|
||||
return nrm.normalize();
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
|
||||
@@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE long btVector3::maxDot( const btVector3 *array, long arra
|
||||
if( array_count < scalar_cutoff )
|
||||
#endif
|
||||
{
|
||||
btScalar maxDot = -SIMD_INFINITY;
|
||||
btScalar maxDot1 = -SIMD_INFINITY;
|
||||
int i = 0;
|
||||
int ptIndex = -1;
|
||||
for( i = 0; i < array_count; i++ )
|
||||
{
|
||||
btScalar dot = array[i].dot(*this);
|
||||
|
||||
if( dot > maxDot )
|
||||
if( dot > maxDot1 )
|
||||
{
|
||||
maxDot = dot;
|
||||
maxDot1 = dot;
|
||||
ptIndex = i;
|
||||
}
|
||||
}
|
||||
|
||||
dotOut = maxDot;
|
||||
dotOut = maxDot1;
|
||||
return ptIndex;
|
||||
}
|
||||
#if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
|
||||
|
||||
Reference in New Issue
Block a user