17 #ifndef BT_LEMKE_SOLVER_H
18 #define BT_LEMKE_SOLVER_H
56 for (
int row = 0; row < n; row++)
71 A1.resize(
A.rows(),
A.cols());
72 for (
int row = 0; row <
A.rows(); row++)
81 matrix.resize(n, 2 * n);
82 for (
int row = 0; row < n; row++)
86 matrix.setElem(row,
col,
A1(row,
col));
92 for (i = 0; i < n; i++)
94 for (j = n; j < 2 * n; j++)
97 matrix.setElem(i, j, 1.0);
99 matrix.setElem(i, j, 0.0);
102 for (i = 0; i < n; i++)
104 for (j = 0; j < n; j++)
113 ratio = matrix(j, i) / matrix(i, i);
114 for (k = 0; k < 2 * n; k++)
116 matrix.addElem(j, k, -ratio * matrix(i, k));
121 for (i = 0; i < n; i++)
129 for (j = 0; j < 2 * n; j++)
131 matrix.mulElem(i, j, invA);
135 for (
int row = 0; row < n; row++)
139 B.setElem(row,
col, matrix(row, n +
col));
147 for (
int row = 0; row < n; row++)
149 b1.setElem(row, 0, -
b[row]);
153 M.setElem(row,
col,
v);
154 M.setElem(n + row, n +
col,
v);
155 M.setElem(n + row,
col, -
v);
156 M.setElem(row, n +
col, -
v);
165 for (
int row = 0; row < n; row++)
167 qq[row] = -Bb1(row, 0) - lo[row];
168 qq[n + row] = Bb1(row, 0) + hi[row];
181 for (
int row = 0; row < n; row++)
183 y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
186 for (
int i = 0; i < n; i++)
188 y1_b1.setElem(i, 0,
y1(i, 0) - b1(i, 0));
195 for (
int row = 0; row < n; row++)
197 solution[row] = x1(row, 0);
200 int errorIndexMax = -1;
201 int errorIndexMin = -1;
202 float errorValueMax = -1e30;
203 float errorValueMin = 1e30;
205 for (
int i = 0; i < n; i++)
220 if (
x[i] > errorValueMax)
224 errorValueMax =
x[i];
230 if (
x[i] < errorValueMin)
233 errorValueMin =
x[i];
241 int m_errorCountTimes = 0;
242 if (errorIndexMin < 0)
244 if (errorIndexMax < 0)
248 for (
int i = 0; i < n; i++)
258 int dimension =
A.rows();
266 for (
int row = 0; row < dimension; row++)
280 int errorIndexMax = -1;
281 int errorIndexMin = -1;
282 float errorValueMax = -1e30;
283 float errorValueMin = 1e30;
285 for (
int i = 0; i < dimension; i++)
287 x[i] = solution[i + dimension];
299 if (
x[i] > errorValueMax)
303 errorValueMax =
x[i];
309 if (
x[i] < errorValueMin)
312 errorValueMin =
x[i];
320 static int errorCountTimes = 0;
321 if (errorIndexMin < 0)
323 if (errorIndexMax < 0)
325 printf(
"Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
326 for (
int i = 0; i < dimension; i++)
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble y1
ATTR_WARN_UNUSED_RESULT const BMVert * v
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simula...
void setSystem(const btMatrixXu &M_, const btVectorXu &q_)
set system with Matrix M and vector q
original version written by Erwin Coumans, October 2013
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)
original version written by Erwin Coumans, October 2013
static const pxr::TfToken b("b", pxr::TfToken::Immortal)