diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h | 151 |
1 files changed, 151 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.h new file mode 100644 index 0000000000..9ec31a6d4e --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btPATHSolver.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 |