mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-07 13:57:48 +00:00
GEEKING
This commit is contained in:
watrabi
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,77 @@
|
||||
/*************************************************************************
|
||||
* *
|
||||
* 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"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
struct btDantzigScratchMemory
|
||||
{
|
||||
btAlignedObjectArray<btScalar> m_scratch;
|
||||
btAlignedObjectArray<btScalar> L;
|
||||
btAlignedObjectArray<btScalar> d;
|
||||
btAlignedObjectArray<btScalar> delta_w;
|
||||
btAlignedObjectArray<btScalar> delta_x;
|
||||
btAlignedObjectArray<btScalar> Dell;
|
||||
btAlignedObjectArray<btScalar> ell;
|
||||
btAlignedObjectArray<btScalar*> Arows;
|
||||
btAlignedObjectArray<int> p;
|
||||
btAlignedObjectArray<int> C;
|
||||
btAlignedObjectArray<bool> state;
|
||||
};
|
||||
|
||||
//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,btDantzigScratchMemory& scratch);
|
||||
|
||||
|
||||
|
||||
#endif //_BT_LCP_H_
|
||||
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
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
|
||||
@@ -0,0 +1,627 @@
|
||||
/*
|
||||
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
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "btMLCPSolver.h"
|
||||
#include "LinearMath/btMatrixX.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSolveProjectedGaussSeidel.h"
|
||||
|
||||
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
|
||||
:m_solver(solver),
|
||||
m_fallback(0)
|
||||
{
|
||||
}
|
||||
|
||||
btMLCPSolver::~btMLCPSolver()
|
||||
{
|
||||
}
|
||||
|
||||
bool gUseMatrixMultiply = false;
|
||||
bool interleaveContactAndFriction = false;
|
||||
|
||||
btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
|
||||
{
|
||||
BT_PROFILE("gather constraint data");
|
||||
|
||||
int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
|
||||
|
||||
|
||||
int numBodies = m_tmpSolverBodyPool.size();
|
||||
m_allConstraintArray.resize(0);
|
||||
m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
|
||||
btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
|
||||
// printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
|
||||
|
||||
int dindex = 0;
|
||||
for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
|
||||
{
|
||||
m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
|
||||
m_limitDependencies[dindex++] = -1;
|
||||
}
|
||||
|
||||
///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
|
||||
|
||||
int firstContactConstraintOffset=dindex;
|
||||
|
||||
if (interleaveContactAndFriction)
|
||||
{
|
||||
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
|
||||
{
|
||||
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
|
||||
m_limitDependencies[dindex++] = -1;
|
||||
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
|
||||
int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
|
||||
m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
|
||||
if (numFrictionPerContact==2)
|
||||
{
|
||||
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
|
||||
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
|
||||
{
|
||||
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
|
||||
m_limitDependencies[dindex++] = -1;
|
||||
}
|
||||
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
|
||||
{
|
||||
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
|
||||
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (!m_allConstraintArray.size())
|
||||
{
|
||||
m_A.resize(0,0);
|
||||
m_b.resize(0);
|
||||
m_x.resize(0);
|
||||
m_lo.resize(0);
|
||||
m_hi.resize(0);
|
||||
return 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (gUseMatrixMultiply)
|
||||
{
|
||||
BT_PROFILE("createMLCP");
|
||||
createMLCP(infoGlobal);
|
||||
}
|
||||
else
|
||||
{
|
||||
BT_PROFILE("createMLCPFast");
|
||||
createMLCPFast(infoGlobal);
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
bool result = true;
|
||||
|
||||
if (m_A.rows()==0)
|
||||
return true;
|
||||
|
||||
//if using split impulse, we solve 2 separate (M)LCPs
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
btMatrixXu Acopy = m_A;
|
||||
btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
|
||||
// printf("solve first LCP\n");
|
||||
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
|
||||
if (result)
|
||||
result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
|
||||
|
||||
} else
|
||||
{
|
||||
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
struct btJointNode
|
||||
{
|
||||
int jointIndex; // pointer to enclosing dxJoint object
|
||||
int otherBodyIndex; // *other* body this joint is connected to
|
||||
int nextJointNodeIndex;//-1 for null
|
||||
int constraintRowIndex;
|
||||
};
|
||||
|
||||
|
||||
|
||||
void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numContactRows = interleaveContactAndFriction ? 3 : 1;
|
||||
|
||||
int numConstraintRows = m_allConstraintArray.size();
|
||||
int n = numConstraintRows;
|
||||
{
|
||||
BT_PROFILE("init b (rhs)");
|
||||
m_b.resize(numConstraintRows);
|
||||
m_bSplit.resize(numConstraintRows);
|
||||
//m_b.setZero();
|
||||
for (int i=0;i<numConstraintRows ;i++)
|
||||
{
|
||||
if (m_allConstraintArray[i].m_jacDiagABInv)
|
||||
{
|
||||
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
btScalar* w = 0;
|
||||
int nub = 0;
|
||||
|
||||
m_lo.resize(numConstraintRows);
|
||||
m_hi.resize(numConstraintRows);
|
||||
|
||||
{
|
||||
BT_PROFILE("init lo/ho");
|
||||
|
||||
for (int i=0;i<numConstraintRows;i++)
|
||||
{
|
||||
if (0)//m_limitDependencies[i]>=0)
|
||||
{
|
||||
m_lo[i] = -BT_INFINITY;
|
||||
m_hi[i] = BT_INFINITY;
|
||||
} else
|
||||
{
|
||||
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
|
||||
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
int m=m_allConstraintArray.size();
|
||||
|
||||
int numBodies = m_tmpSolverBodyPool.size();
|
||||
btAlignedObjectArray<int> bodyJointNodeArray;
|
||||
{
|
||||
BT_PROFILE("bodyJointNodeArray.resize");
|
||||
bodyJointNodeArray.resize(numBodies,-1);
|
||||
}
|
||||
btAlignedObjectArray<btJointNode> jointNodeArray;
|
||||
{
|
||||
BT_PROFILE("jointNodeArray.reserve");
|
||||
jointNodeArray.reserve(2*m_allConstraintArray.size());
|
||||
}
|
||||
|
||||
static btMatrixXu J3;
|
||||
{
|
||||
BT_PROFILE("J3.resize");
|
||||
J3.resize(2*m,8);
|
||||
}
|
||||
static btMatrixXu JinvM3;
|
||||
{
|
||||
BT_PROFILE("JinvM3.resize/setZero");
|
||||
|
||||
JinvM3.resize(2*m,8);
|
||||
JinvM3.setZero();
|
||||
J3.setZero();
|
||||
}
|
||||
int cur=0;
|
||||
int rowOffset = 0;
|
||||
static btAlignedObjectArray<int> ofs;
|
||||
{
|
||||
BT_PROFILE("ofs resize");
|
||||
ofs.resize(0);
|
||||
ofs.resizeNoInitialize(m_allConstraintArray.size());
|
||||
}
|
||||
{
|
||||
BT_PROFILE("Compute J and JinvM");
|
||||
int c=0;
|
||||
|
||||
int numRows = 0;
|
||||
|
||||
for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
|
||||
{
|
||||
ofs[c] = rowOffset;
|
||||
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
|
||||
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
|
||||
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
|
||||
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
|
||||
|
||||
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
|
||||
if (orgBodyA)
|
||||
{
|
||||
{
|
||||
int slotA=-1;
|
||||
//find free jointNode slot for sbA
|
||||
slotA =jointNodeArray.size();
|
||||
jointNodeArray.expand();//NonInitializing();
|
||||
int prevSlot = bodyJointNodeArray[sbA];
|
||||
bodyJointNodeArray[sbA] = slotA;
|
||||
jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
|
||||
jointNodeArray[slotA].jointIndex = c;
|
||||
jointNodeArray[slotA].constraintRowIndex = i;
|
||||
jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
|
||||
}
|
||||
for (int row=0;row<numRows;row++,cur++)
|
||||
{
|
||||
btVector3 normalInvMass = m_allConstraintArray[i+row].m_contactNormal1 * orgBodyA->getInvMass();
|
||||
btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[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]);
|
||||
JinvM3.setElem(cur,r,normalInvMass[r]);
|
||||
JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
|
||||
}
|
||||
J3.setElem(cur,3,0);
|
||||
JinvM3.setElem(cur,3,0);
|
||||
J3.setElem(cur,7,0);
|
||||
JinvM3.setElem(cur,7,0);
|
||||
}
|
||||
} else
|
||||
{
|
||||
cur += numRows;
|
||||
}
|
||||
if (orgBodyB)
|
||||
{
|
||||
|
||||
{
|
||||
int slotB=-1;
|
||||
//find free jointNode slot for sbA
|
||||
slotB =jointNodeArray.size();
|
||||
jointNodeArray.expand();//NonInitializing();
|
||||
int prevSlot = bodyJointNodeArray[sbB];
|
||||
bodyJointNodeArray[sbB] = slotB;
|
||||
jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
|
||||
jointNodeArray[slotB].jointIndex = c;
|
||||
jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
|
||||
jointNodeArray[slotB].constraintRowIndex = i;
|
||||
}
|
||||
|
||||
for (int row=0;row<numRows;row++,cur++)
|
||||
{
|
||||
btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
|
||||
btVector3 relPosInvInertiaB = m_allConstraintArray[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]);
|
||||
JinvM3.setElem(cur,r,normalInvMassB[r]);
|
||||
JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
|
||||
}
|
||||
J3.setElem(cur,3,0);
|
||||
JinvM3.setElem(cur,3,0);
|
||||
J3.setElem(cur,7,0);
|
||||
JinvM3.setElem(cur,7,0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
cur += numRows;
|
||||
}
|
||||
rowOffset+=numRows;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//compute JinvM = J*invM.
|
||||
const btScalar* JinvM = JinvM3.getBufferPointer();
|
||||
|
||||
const btScalar* Jptr = J3.getBufferPointer();
|
||||
{
|
||||
BT_PROFILE("m_A.resize");
|
||||
m_A.resize(n,n);
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("m_A.setZero");
|
||||
m_A.setZero();
|
||||
}
|
||||
int c=0;
|
||||
{
|
||||
int numRows = 0;
|
||||
BT_PROFILE("Compute A");
|
||||
for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
|
||||
{
|
||||
int row__ = ofs[c];
|
||||
int sbA = m_allConstraintArray[i].m_solverBodyIdA;
|
||||
int sbB = m_allConstraintArray[i].m_solverBodyIdB;
|
||||
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
|
||||
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
|
||||
|
||||
numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ;
|
||||
|
||||
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
|
||||
|
||||
{
|
||||
int startJointNodeA = bodyJointNodeArray[sbA];
|
||||
while (startJointNodeA>=0)
|
||||
{
|
||||
int j0 = jointNodeArray[startJointNodeA].jointIndex;
|
||||
int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
|
||||
if (j0<c)
|
||||
{
|
||||
|
||||
int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
|
||||
size_t ofsother = (m_allConstraintArray[cr0].m_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]);
|
||||
}
|
||||
startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int startJointNodeB = bodyJointNodeArray[sbB];
|
||||
while (startJointNodeB>=0)
|
||||
{
|
||||
int j1 = jointNodeArray[startJointNodeB].jointIndex;
|
||||
int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
|
||||
|
||||
if (j1<c)
|
||||
{
|
||||
int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
|
||||
size_t ofsother = (m_allConstraintArray[cj1].m_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]);
|
||||
}
|
||||
startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("compute diagonal");
|
||||
// compute diagonal blocks of m_A
|
||||
|
||||
int row__ = 0;
|
||||
int numJointRows = m_allConstraintArray.size();
|
||||
|
||||
int jj=0;
|
||||
for (;row__<numJointRows;)
|
||||
{
|
||||
|
||||
int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
|
||||
int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
|
||||
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
|
||||
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
|
||||
|
||||
|
||||
const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
|
||||
|
||||
const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
|
||||
const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
|
||||
m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
|
||||
if (orgBodyB)
|
||||
{
|
||||
m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__);
|
||||
}
|
||||
row__ += infom;
|
||||
jj++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
///todo: use proper cfm values from the constraints (getInfo2)
|
||||
if (1)
|
||||
{
|
||||
// add cfm to the diagonal of m_A
|
||||
for ( int i=0; i<m_A.rows(); ++i)
|
||||
{
|
||||
float cfm = 0.00001f;
|
||||
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
///fill the upper triangle of the matrix, to make it symmetric
|
||||
{
|
||||
BT_PROFILE("fill the upper triangle ");
|
||||
m_A.copyLowerToUpperTriangle();
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("resize/init x");
|
||||
m_x.resize(numConstraintRows);
|
||||
m_xSplit.resize(numConstraintRows);
|
||||
|
||||
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
for (int i=0;i<m_allConstraintArray.size();i++)
|
||||
{
|
||||
const btSolverConstraint& c = m_allConstraintArray[i];
|
||||
m_x[i]=c.m_appliedImpulse;
|
||||
m_xSplit[i] = c.m_appliedPushImpulse;
|
||||
}
|
||||
} else
|
||||
{
|
||||
m_x.setZero();
|
||||
m_xSplit.setZero();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numBodies = this->m_tmpSolverBodyPool.size();
|
||||
int numConstraintRows = m_allConstraintArray.size();
|
||||
|
||||
m_b.resize(numConstraintRows);
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_bSplit.resize(numConstraintRows);
|
||||
|
||||
for (int i=0;i<numConstraintRows ;i++)
|
||||
{
|
||||
if (m_allConstraintArray[i].m_jacDiagABInv)
|
||||
{
|
||||
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
}
|
||||
}
|
||||
|
||||
static btMatrixXu Minv;
|
||||
Minv.resize(6*numBodies,6*numBodies);
|
||||
Minv.setZero();
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
const btSolverBody& rb = m_tmpSolverBodyPool[i];
|
||||
const btVector3& invMass = rb.m_invMass;
|
||||
setElem(Minv,i*6+0,i*6+0,invMass[0]);
|
||||
setElem(Minv,i*6+1,i*6+1,invMass[1]);
|
||||
setElem(Minv,i*6+2,i*6+2,invMass[2]);
|
||||
btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
|
||||
|
||||
for (int r=0;r<3;r++)
|
||||
for (int c=0;c<3;c++)
|
||||
setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
|
||||
}
|
||||
|
||||
static btMatrixXu J;
|
||||
J.resize(numConstraintRows,6*numBodies);
|
||||
J.setZero();
|
||||
|
||||
m_lo.resize(numConstraintRows);
|
||||
m_hi.resize(numConstraintRows);
|
||||
|
||||
for (int i=0;i<numConstraintRows;i++)
|
||||
{
|
||||
|
||||
m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
|
||||
m_hi[i] = m_allConstraintArray[i].m_upperLimit;
|
||||
|
||||
int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
|
||||
int bodyIndex1 = m_allConstraintArray[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]);
|
||||
}
|
||||
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]);
|
||||
}
|
||||
}
|
||||
|
||||
static btMatrixXu J_transpose;
|
||||
J_transpose= J.transpose();
|
||||
|
||||
static btMatrixXu tmp;
|
||||
|
||||
{
|
||||
{
|
||||
BT_PROFILE("J*Minv");
|
||||
tmp = J*Minv;
|
||||
|
||||
}
|
||||
{
|
||||
BT_PROFILE("J*tmp");
|
||||
m_A = tmp*J_transpose;
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
float cfm = 0.0001f;
|
||||
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
m_x.resize(numConstraintRows);
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_xSplit.resize(numConstraintRows);
|
||||
// m_x.setZero();
|
||||
|
||||
for (int i=0;i<m_allConstraintArray.size();i++)
|
||||
{
|
||||
const btSolverConstraint& c = m_allConstraintArray[i];
|
||||
m_x[i]=c.m_appliedImpulse;
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_xSplit[i] = c.m_appliedPushImpulse;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
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());
|
||||
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++)
|
||||
{
|
||||
{
|
||||
btSolverConstraint& c = m_allConstraintArray[i];
|
||||
int sbA = c.m_solverBodyIdA;
|
||||
int sbB = c.m_solverBodyIdB;
|
||||
btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
|
||||
btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
|
||||
|
||||
btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
|
||||
btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
|
||||
|
||||
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
|
||||
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
|
||||
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
|
||||
c.m_appliedPushImpulse = m_xSplit[i];
|
||||
}
|
||||
c.m_appliedImpulse = m_x[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_fallback++;
|
||||
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
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_MLCP_SOLVER_H
|
||||
#define BT_MLCP_SOLVER_H
|
||||
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "LinearMath/btMatrixX.h"
|
||||
#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
|
||||
|
||||
class btMLCPSolver : public btSequentialImpulseConstraintSolver
|
||||
{
|
||||
|
||||
protected:
|
||||
|
||||
btMatrixXu m_A;
|
||||
btVectorXu m_b;
|
||||
btVectorXu m_x;
|
||||
btVectorXu m_lo;
|
||||
btVectorXu m_hi;
|
||||
|
||||
///when using 'split impulse' we solve two separate (M)LCPs
|
||||
btVectorXu m_bSplit;
|
||||
btVectorXu m_xSplit;
|
||||
btVectorXu m_bSplit1;
|
||||
btVectorXu m_xSplit2;
|
||||
|
||||
btAlignedObjectArray<int> m_limitDependencies;
|
||||
btConstraintArray m_allConstraintArray;
|
||||
btMLCPSolverInterface* m_solver;
|
||||
int m_fallback;
|
||||
|
||||
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);
|
||||
|
||||
//return true is it solves the problem successfully
|
||||
virtual bool solveMLCP(const btContactSolverInfo& infoGlobal);
|
||||
|
||||
public:
|
||||
|
||||
btMLCPSolver( btMLCPSolverInterface* solver);
|
||||
virtual ~btMLCPSolver();
|
||||
|
||||
void setMLCPSolver(btMLCPSolverInterface* solver)
|
||||
{
|
||||
m_solver = solver;
|
||||
}
|
||||
|
||||
int getNumFallbacks() const
|
||||
{
|
||||
return m_fallback;
|
||||
}
|
||||
void setNumFallbacks(int num)
|
||||
{
|
||||
m_fallback = num;
|
||||
}
|
||||
|
||||
virtual btConstraintSolverType getSolverType() const
|
||||
{
|
||||
return BT_MLCP_SOLVER;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //BT_MLCP_SOLVER_H
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
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_MLCP_SOLVER_INTERFACE_H
|
||||
#define BT_MLCP_SOLVER_INTERFACE_H
|
||||
|
||||
#include "LinearMath/btMatrixX.h"
|
||||
|
||||
class btMLCPSolverInterface
|
||||
{
|
||||
public:
|
||||
virtual ~btMLCPSolverInterface()
|
||||
{
|
||||
}
|
||||
|
||||
//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
|
||||
@@ -0,0 +1,151 @@
|
||||
/*
|
||||
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_PATH_SOLVER_H
|
||||
#define BT_PATH_SOLVER_H
|
||||
|
||||
//#define BT_USE_PATH
|
||||
#ifdef BT_USE_PATH
|
||||
|
||||
extern "C" {
|
||||
#include "PATH/SimpleLCP.h"
|
||||
#include "PATH/License.h"
|
||||
#include "PATH/Error_Interface.h"
|
||||
};
|
||||
void __stdcall MyError(Void *data, Char *msg)
|
||||
{
|
||||
printf("Path Error: %s\n",msg);
|
||||
}
|
||||
void __stdcall MyWarning(Void *data, Char *msg)
|
||||
{
|
||||
printf("Path Warning: %s\n",msg);
|
||||
}
|
||||
|
||||
Error_Interface e;
|
||||
|
||||
|
||||
|
||||
#include "btMLCPSolverInterface.h"
|
||||
#include "Dantzig/lcp.h"
|
||||
|
||||
class btPathSolver : public btMLCPSolverInterface
|
||||
{
|
||||
public:
|
||||
|
||||
btPathSolver()
|
||||
{
|
||||
License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
|
||||
e.error_data = 0;
|
||||
e.warning = MyWarning;
|
||||
e.error = MyError;
|
||||
Error_SetInterface(&e);
|
||||
}
|
||||
|
||||
|
||||
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 true;
|
||||
|
||||
/* - variables - the number of variables in the problem
|
||||
- m_nnz - the number of nonzeros in the M matrix
|
||||
- m_i - a vector of size m_nnz containing the row indices for M
|
||||
- m_j - a vector of size m_nnz containing the column indices for M
|
||||
- m_ij - a vector of size m_nnz containing the data for M
|
||||
- q - a vector of size variables
|
||||
- lb - a vector of size variables containing the lower bounds on x
|
||||
- ub - a vector of size variables containing the upper bounds on x
|
||||
*/
|
||||
btAlignedObjectArray<double> values;
|
||||
btAlignedObjectArray<int> rowIndices;
|
||||
btAlignedObjectArray<int> colIndices;
|
||||
|
||||
for (int i=0;i<A.rows();i++)
|
||||
{
|
||||
for (int j=0;j<A.cols();j++)
|
||||
{
|
||||
if (A(i,j)!=0.f)
|
||||
{
|
||||
//add 1, because Path starts at 1, instead of 0
|
||||
rowIndices.push_back(i+1);
|
||||
colIndices.push_back(j+1);
|
||||
values.push_back(A(i,j));
|
||||
}
|
||||
}
|
||||
}
|
||||
int numNonZero = rowIndices.size();
|
||||
btAlignedObjectArray<double> zResult;
|
||||
zResult.resize(numVariables);
|
||||
btAlignedObjectArray<double> rhs;
|
||||
btAlignedObjectArray<double> upperBounds;
|
||||
btAlignedObjectArray<double> lowerBounds;
|
||||
for (int i=0;i<numVariables;i++)
|
||||
{
|
||||
upperBounds.push_back(hi[i]);
|
||||
lowerBounds.push_back(lo[i]);
|
||||
rhs.push_back(-b[i]);
|
||||
}
|
||||
|
||||
|
||||
SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
|
||||
|
||||
if (status != MCP_Solved)
|
||||
{
|
||||
static const char* gReturnMsgs[] = {
|
||||
"Invalid return",
|
||||
"MCP_Solved: The problem was solved",
|
||||
"MCP_NoProgress: A stationary point was found",
|
||||
"MCP_MajorIterationLimit: Major iteration limit met",
|
||||
"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
|
||||
"MCP_TimeLimit: Ran out of time",
|
||||
"MCP_UserInterrupt: Control-C, typically",
|
||||
"MCP_BoundError: Problem has a bound error",
|
||||
"MCP_DomainError: Could not find starting point",
|
||||
"MCP_Infeasible: Problem has no solution",
|
||||
"MCP_Error: An error occurred within the code",
|
||||
"MCP_LicenseError: License could not be found",
|
||||
"MCP_OK"
|
||||
};
|
||||
|
||||
printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_USE_PATH
|
||||
|
||||
|
||||
#endif //BT_PATH_SOLVER_H
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
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_SOLVE_PROJECTED_GAUSS_SEIDEL_H
|
||||
#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
|
||||
|
||||
|
||||
#include "btMLCPSolverInterface.h"
|
||||
|
||||
class btSolveProjectedGaussSeidel : 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)
|
||||
{
|
||||
//A is a m-n matrix, m rows, n columns
|
||||
btAssert(A.rows() == b.rows());
|
||||
|
||||
int i, j, numRows = A.rows();
|
||||
|
||||
float delta;
|
||||
|
||||
for (int k = 0; k <numIterations; k++)
|
||||
{
|
||||
for (i = 0; i <numRows; i++)
|
||||
{
|
||||
delta = 0.0f;
|
||||
if (useSparsity)
|
||||
{
|
||||
for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
|
||||
{
|
||||
int j = A.m_rowNonZeroElements1[i][h];
|
||||
if (j != i)//skip main diagonal
|
||||
{
|
||||
delta += A(i,j) * x[j];
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
for (j = 0; j <i; j++)
|
||||
delta += A(i,j) * x[j];
|
||||
for (j = i+1; j<numRows; j++)
|
||||
delta += A(i,j) * x[j];
|
||||
}
|
||||
|
||||
float aDiag = A(i,i);
|
||||
x [i] = (b [i] - delta) / A(i,i);
|
||||
float s = 1.f;
|
||||
|
||||
if (limitDependency[i]>=0)
|
||||
{
|
||||
s = x[limitDependency[i]];
|
||||
if (s<0)
|
||||
s=1;
|
||||
}
|
||||
|
||||
if (x[i]<lo[i]*s)
|
||||
x[i]=lo[i]*s;
|
||||
if (x[i]>hi[i]*s)
|
||||
x[i]=hi[i]*s;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
|
||||
Reference in New Issue
Block a user