mirror of https://github.com/axmolengine/axmol.git
339 lines
7.9 KiB
C
339 lines
7.9 KiB
C
|
/*
|
|||
|
|
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<int>& 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; row < n; row++)
|
|||
|
|
{
|
|||
|
|
q1[row] = -b[row];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// cout << "A" << endl;
|
|||
|
|
// cout << A << endl;
|
|||
|
|
|
|||
|
|
/////////////////////////////////////
|
|||
|
|
|
|||
|
|
//slow matrix inversion, replace with LU decomposition
|
|||
|
|
btMatrixXu A1;
|
|||
|
|
btMatrixXu B(n, n);
|
|||
|
|
{
|
|||
|
|
//BT_PROFILE("inverse(slow)");
|
|||
|
|
A1.resize(A.rows(), A.cols());
|
|||
|
|
for (int row = 0; row < A.rows(); row++)
|
|||
|
|
{
|
|||
|
|
for (int col = 0; col < A.cols(); col++)
|
|||
|
|
{
|
|||
|
|
A1.setElem(row, col, A(row, col));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btMatrixXu matrix;
|
|||
|
|
matrix.resize(n, 2 * n);
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
for (int col = 0; col < n; col++)
|
|||
|
|
{
|
|||
|
|
matrix.setElem(row, col, A1(row, col));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btScalar ratio, a;
|
|||
|
|
int i, j, k;
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
for (j = n; j < 2 * n; j++)
|
|||
|
|
{
|
|||
|
|
if (i == (j - n))
|
|||
|
|
matrix.setElem(i, j, 1.0);
|
|||
|
|
else
|
|||
|
|
matrix.setElem(i, j, 0.0);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
for (j = 0; j < n; j++)
|
|||
|
|
{
|
|||
|
|
if (i != j)
|
|||
|
|
{
|
|||
|
|
btScalar v = matrix(i, i);
|
|||
|
|
if (btFuzzyZero(v))
|
|||
|
|
{
|
|||
|
|
a = 0.000001f;
|
|||
|
|
}
|
|||
|
|
ratio = matrix(j, i) / matrix(i, i);
|
|||
|
|
for (k = 0; k < 2 * n; k++)
|
|||
|
|
{
|
|||
|
|
matrix.addElem(j, k, -ratio * matrix(i, k));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
for (i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
a = matrix(i, i);
|
|||
|
|
if (btFuzzyZero(a))
|
|||
|
|
{
|
|||
|
|
a = 0.000001f;
|
|||
|
|
}
|
|||
|
|
btScalar invA = 1.f / a;
|
|||
|
|
for (j = 0; j < 2 * n; j++)
|
|||
|
|
{
|
|||
|
|
matrix.mulElem(i, j, invA);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
for (int col = 0; col < n; col++)
|
|||
|
|
{
|
|||
|
|
B.setElem(row, col, matrix(row, n + col));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btMatrixXu b1(n, 1);
|
|||
|
|
|
|||
|
|
btMatrixXu M(n * 2, n * 2);
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
b1.setElem(row, 0, -b[row]);
|
|||
|
|
for (int col = 0; col < n; col++)
|
|||
|
|
{
|
|||
|
|
btScalar v = B(row, col);
|
|||
|
|
M.setElem(row, col, v);
|
|||
|
|
M.setElem(n + row, n + col, v);
|
|||
|
|
M.setElem(n + row, col, -v);
|
|||
|
|
M.setElem(row, n + col, -v);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btMatrixXu Bb1 = B * b1;
|
|||
|
|
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
|
|||
|
|
|
|||
|
|
btVectorXu qq;
|
|||
|
|
qq.resize(n * 2);
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
qq[row] = -Bb1(row, 0) - lo[row];
|
|||
|
|
qq[n + row] = Bb1(row, 0) + hi[row];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btVectorXu z1;
|
|||
|
|
|
|||
|
|
btMatrixXu y1;
|
|||
|
|
y1.resize(n, 1);
|
|||
|
|
btLemkeAlgorithm lemke(M, qq, m_debugLevel);
|
|||
|
|
{
|
|||
|
|
//BT_PROFILE("lemke.solve");
|
|||
|
|
lemke.setSystem(M, qq);
|
|||
|
|
z1 = lemke.solve(m_maxLoops);
|
|||
|
|
}
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
|
|||
|
|
}
|
|||
|
|
btMatrixXu y1_b1(n, 1);
|
|||
|
|
for (int i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btMatrixXu x1;
|
|||
|
|
|
|||
|
|
x1 = B * (y1_b1);
|
|||
|
|
|
|||
|
|
for (int row = 0; row < n; row++)
|
|||
|
|
{
|
|||
|
|
solution[row] = x1(row, 0); //n];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int errorIndexMax = -1;
|
|||
|
|
int errorIndexMin = -1;
|
|||
|
|
float errorValueMax = -1e30;
|
|||
|
|
float errorValueMin = 1e30;
|
|||
|
|
|
|||
|
|
for (int i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
x[i] = solution[i];
|
|||
|
|
volatile btScalar check = x[i];
|
|||
|
|
if (x[i] != check)
|
|||
|
|
{
|
|||
|
|
//printf("Lemke result is #NAN\n");
|
|||
|
|
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] > 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] < errorValueMin)
|
|||
|
|
{
|
|||
|
|
errorIndexMin = i;
|
|||
|
|
errorValueMin = x[i];
|
|||
|
|
fail = true;
|
|||
|
|
//printf("x[i] = %f,",x[i]);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (fail)
|
|||
|
|
{
|
|||
|
|
int m_errorCountTimes = 0;
|
|||
|
|
if (errorIndexMin < 0)
|
|||
|
|
errorValueMin = 0.f;
|
|||
|
|
if (errorIndexMax < 0)
|
|||
|
|
errorValueMax = 0.f;
|
|||
|
|
m_errorCountTimes++;
|
|||
|
|
// printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
|
|||
|
|
for (int i = 0; i < n; i++)
|
|||
|
|
{
|
|||
|
|
x[i] = 0.f;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return !fail;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
|
|||
|
|
{
|
|||
|
|
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];
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
btLemkeAlgorithm lemke(A, q, m_debugLevel);
|
|||
|
|
|
|||
|
|
lemke.setSystem(A, q);
|
|||
|
|
|
|||
|
|
btVectorXu solution = lemke.solve(m_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] > 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] < 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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return !fail;
|
|||
|
|
}
|
|||
|
|
return true;
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
#endif //BT_LEMKE_SOLVER_H
|