Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Added Dantzig MLCP solver option from Open Dynamics Engine (trying to avoid naming/linking conflicts in case ODE and Bullet is used together)

Browse Source 
If an MLCP solver fails, use PGS/SI fallback, add a boolean return value for 'solve' method
This commit is contained in:
erwin.coumans@gmail.com
2013-10-21 23:27:09 +00:00
parent 1ca0493dc4
commit 379f0079e0
10 changed files with 2322 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2094
src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

61
src/BulletDynamics/MLCPSolvers/btDantzigLCP.h Normal file
View File

@@ -0,0 +1,61 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of *
* The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
/*
given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
satisfies one of
(1) x = lo, w >= 0
(2) x = hi, w <= 0
(3) lo < x < hi, w = 0
A is a matrix of dimension n*n, everything else is a vector of size n*1.
lo and hi can be +/- dInfinity as needed. the first `nub' variables are
unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
we restrict lo(i) <= 0 and hi(i) >= 0.
the original data (A,b) may be modified by this function.
if the `findex' (friction index) parameter is nonzero, it points to an array
of index values. in this case constraints that have findex[i] >= 0 are
special. all non-special constraints are solved for, then the lo and hi values
for the special constraints are set:
hi[i] = abs( hi[i] * x[findex[i]] )
lo[i] = -hi[i]
and the solution continues. this mechanism allows a friction approximation
to be implemented. the first `nub' variables are assumed to have findex < 0.
*/
#ifndef _BT_LCP_H_
#define _BT_LCP_H_
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include "LinearMath/btScalar.h"
//return false if solving failed
bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b, btScalar *w,
int nub, btScalar *lo, btScalar *hi, int *findex);
#endif //_BT_LCP_H_

103
src/BulletDynamics/MLCPSolvers/btDantzigSolver.h Normal file
View File

@@ -0,0 +1,103 @@
/*
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_DANTZIG_SOLVER_H
#define BT_DANTZIG_SOLVER_H
#include "btMLCPSolverInterface.h"
#include "btDantzigLCP.h"
class btDantzigSolver : public btMLCPSolverInterface
{
protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
public:
btDantzigSolver()
:m_acceptableUpperLimitSolution(btScalar(1000))
{
}
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)
{
bool result = true;
int n = b.rows();
if (n)
{
btScalar* AA = (btScalar*) A.getBufferPointer();
btScalar* bb = (btScalar* ) b.getBufferPointer();
btScalar* xx = (btScalar*) x.getBufferPointer();
btScalar* llo = (btScalar*) lo.getBufferPointer();
btScalar* hhi = (btScalar*) hi.getBufferPointer();
int* findex = (int*) &limitDependency[0];
int nub = 0;
btAlignedObjectArray<btScalar> ww;
ww.resize(n);
const btScalar* Aptr = A.getBufferPointer();
for (int i=0;i<n*n;i++)
{
AA[i] = Aptr[i];
}
for (int i=0;i<n;i++)
{
llo[i] = lo[i];
hhi[i] = hi[i];
bb[i] = b[i];
xx[i] = x[i];
}
extern int numAllocas;
numAllocas = 0;
result = btSolveDantzigLCP (n,AA,xx,bb,&ww[0],nub,llo,hhi,findex);
// printf("numAllocas = %d\n",numAllocas);
for (int i=0;i<n;i++)
{
x[i] = xx[i];
//test for NAN
if (x[i] != xx[i])
{
return false;
}
if (x[i] >= m_acceptableUpperLimitSolution)
{
return false;
}
if (x[i] <= -m_acceptableUpperLimitSolution)
{
return false;
}
}
}
return result;
}
};
#endif //BT_DANTZIG_SOLVER_H

21
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@@ -115,10 +115,9 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
return 0.f;
}
void btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
return m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
}
struct btJointNode
@@ -400,7 +399,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
}
}
}
///todo: use proper cfm values from the constraints (getInfo2)
if (1)
{
// add cfm to the diagonal of m_A
@@ -520,6 +520,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
if (1)
{
///todo: use proper cfm values from the constraints (getInfo2)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
@@ -543,13 +544,15 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
bool result = true;
{
BT_PROFILE("solveMLCP");
// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
solveMLCP(infoGlobal);
result = solveMLCP(infoGlobal);
}
//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
if (result)
{
BT_PROFILE("process MLCP results");
for (int i=0;i<m_allConstraintArray.size();i++)
@@ -570,9 +573,11 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
}
}
}
//btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
else
{
printf("fallback to btSequentialImpulseConstraintSolver\n");
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}
return 0.f;
}

3
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
View File

@@ -43,7 +43,8 @@ protected:
virtual void createMLCP(const btContactSolverInfo& infoGlobal);
virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
virtual void solveMLCP(const btContactSolverInfo& infoGlobal);
//return true is it solves the problem successfully
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
public:

3
src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
View File

@@ -26,7 +26,8 @@ public:
{
}
virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
//return true is it solves the problem successfully
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)=0;
};
#endif //BT_MLCP_SOLVER_INTERFACE_H

14
src/BulletDynamics/MLCPSolvers/btPATHSolver.h
View File

@@ -56,14 +56,14 @@ public:
}
virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
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)
{
MCP_Termination status;
int numVariables = b.rows();
if (0==numVariables)
return;
return true;
/* - variables - the number of variables in the problem
- m_nnz - the number of nonzeros in the M matrix
@@ -126,14 +126,20 @@ public:
};
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
printf("using Projected Gauss Seidel instead\n");
//x = Solve_GaussSeidel(A,b,lo,hi,limitDependencies,infoGlobal.m_numIterations);
printf("using Projected Gauss Seidel fallback\n");
return false;
} else
{
for (int i=0;i<numVariables;i++)
{
x[i] = zResult[i];
//check for #NAN
if (x[i] != zResult[i])
return false;
}
return true;
}
}

2
src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
View File

@@ -23,7 +23,7 @@ subject to the following restrictions:
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{
public:
virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
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)
{
//A is a m-n matrix, m rows, n columns
btAssert(A.rows() == b.rows());

38
src/LinearMath/btMatrixX.h
View File

@@ -131,46 +131,10 @@ struct btMatrixX
{
if (m_storage[col+row*m_cols]==0.f)
{
m_rowNonZeroElements1[row].push_back(col);
m_colNonZeroElements[col].push_back(row);
/*
//we need to keep the m_rowNonZeroElements1/m_colNonZeroElements arrays sorted (it is too slow, so commented out)
int f=0;
int l=0;
m_rowNonZeroElements1[row].findBinarySearch(col,&f,&l);
// btAssert(f==l);
if (f<m_rowNonZeroElements1[row].size()-1)
{
m_rowNonZeroElements1[row].expandNonInitializing();
for (int j=m_rowNonZeroElements1[row].size()-1;j>f;j--)
m_rowNonZeroElements1[row][j] = m_rowNonZeroElements1[row][j-1];
m_rowNonZeroElements1[row][f] = col;
} else
{
m_rowNonZeroElements1[row].push_back(col);
}
m_colNonZeroElements[col].findBinarySearch(row,&f,&l);
// btAssert(f==l);
if (f<m_colNonZeroElements[col].size()-1)
{
m_colNonZeroElements[col].expandNonInitializing();
for (int j=m_colNonZeroElements[col].size()-1;j>f;j++)
m_colNonZeroElements[col][j-1] = m_colNonZeroElements[col][j];
m_colNonZeroElements[col][f] = row;
} else
{
m_colNonZeroElements[col].push_back(row);
}
*/
m_storage[row*m_cols+col] = val;
} else
{
}
m_storage[row*m_cols+col] = val;
}
}
const T& operator() (int row,int col) const

39
src/LinearMath/btScalar.h
View File

@@ -257,10 +257,12 @@ inline int btGetVersion()
///The btScalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(BT_USE_DOUBLE_PRECISION)
typedef double btScalar;
//this number could be bigger in double precision
#define BT_LARGE_FLOAT 1e30
#else
typedef float btScalar;
//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
#define BT_LARGE_FLOAT 1e18f
@@ -412,15 +414,15 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
#endif
#define SIMD_2_PI btScalar(6.283185307179586232)
#define SIMD_PI (SIMD_2_PI * btScalar(0.5))
#define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25))
#define SIMD_PI btScalar(3.1415926535897932384626433832795029)
#define SIMD_2_PI btScalar(2.0) * SIMD_PI
#define SIMD_HALF_PI (SIMD_PI * btScalar(0.5))
#define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
#define SIMD_DEGS_PER_RAD (btScalar(360.0) / SIMD_2_PI)
#define SIMDSQRT12 btScalar(0.7071067811865475244008443621048490)
#define btRecipSqrt(x) ((btScalar)(btScalar(1.0)/btSqrt(btScalar(x)))) /* reciprocal square root */
#define btRecip(x) (btScalar(1.0)/btScalar(x))
#ifdef BT_USE_DOUBLE_PRECISION
#define SIMD_EPSILON DBL_EPSILON
@@ -622,6 +624,35 @@ SIMD_FORCE_INLINE void btSetZero(T* a, int n)
--ncurr;
}
}
SIMD_FORCE_INLINE btScalar btLargeDot(const btScalar *a, const btScalar *b, int n)
{
btScalar p0,q0,m0,p1,q1,m1,sum;
sum = 0;
n -= 2;
while (n >= 0) {
p0 = a[0]; q0 = b[0];
m0 = p0 * q0;
p1 = a[1]; q1 = b[1];
m1 = p1 * q1;
sum += m0;
sum += m1;
a += 2;
b += 2;
n -= 2;
}
n += 2;
while (n > 0) {
sum += (*a) * (*b);
a++;
b++;
n--;
}
return sum;
}
// returns normalized value in range [-SIMD_PI, SIMD_PI]
SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
{