113 lines
2.9 KiB
C
113 lines
2.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_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;
|
||
|
|
|
||
|
|
btAlignedObjectArray<btScalar> m_A;
|
||
|
|
btAlignedObjectArray<btScalar> m_b;
|
||
|
|
btAlignedObjectArray<btScalar> m_x;
|
||
|
|
btAlignedObjectArray<btScalar> m_lo;
|
||
|
|
btAlignedObjectArray<btScalar> m_hi;
|
||
|
|
btAlignedObjectArray<int> m_dependencies;
|
||
|
|
btDantzigScratchMemory m_scratchMemory;
|
||
|
|
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)
|
||
|
|
{
|
||
|
|
int nub = 0;
|
||
|
|
btAlignedObjectArray<btScalar> ww;
|
||
|
|
ww.resize(n);
|
||
|
|
|
||
|
|
|
||
|
|
const btScalar* Aptr = A.getBufferPointer();
|
||
|
|
m_A.resize(n*n);
|
||
|
|
for (int i=0;i<n*n;i++)
|
||
|
|
{
|
||
|
|
m_A[i] = Aptr[i];
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
m_b.resize(n);
|
||
|
|
m_x.resize(n);
|
||
|
|
m_lo.resize(n);
|
||
|
|
m_hi.resize(n);
|
||
|
|
m_dependencies.resize(n);
|
||
|
|
for (int i=0;i<n;i++)
|
||
|
|
{
|
||
|
|
m_lo[i] = lo[i];
|
||
|
|
m_hi[i] = hi[i];
|
||
|
|
m_b[i] = b[i];
|
||
|
|
m_x[i] = x[i];
|
||
|
|
m_dependencies[i] = limitDependency[i];
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0],m_scratchMemory);
|
||
|
|
if (!result)
|
||
|
|
return result;
|
||
|
|
|
||
|
|
// printf("numAllocas = %d\n",numAllocas);
|
||
|
|
for (int i=0;i<n;i++)
|
||
|
|
{
|
||
|
|
volatile btScalar xx = m_x[i];
|
||
|
|
if (xx != m_x[i])
|
||
|
|
return false;
|
||
|
|
if (x[i] >= m_acceptableUpperLimitSolution)
|
||
|
|
{
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (x[i] <= -m_acceptableUpperLimitSolution)
|
||
|
|
{
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int i=0;i<n;i++)
|
||
|
|
{
|
||
|
|
x[i] = m_x[i];
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif //BT_DANTZIG_SOLVER_H
|