Blender  V3.3
btLemkeAlgorithm.cpp
Go to the documentation of this file.
1 /* Copyright (C) 2004-2013 MBSim Development Team
2 
3 Code was converted for the Bullet Continuous Collision Detection and Physics Library
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 //The original version is here
17 //https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
18 //This file is re-distributed under the ZLib license, with permission of the original author
19 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
20 //STL/std::vector replaced by btAlignedObjectArray
21 
22 #include "btLemkeAlgorithm.h"
23 
24 #undef BT_DEBUG_OSTREAM
25 #ifdef BT_DEBUG_OSTREAM
26 using namespace std;
27 #endif //BT_DEBUG_OSTREAM
28 
30 {
31  static bool calculated = false;
32  static btScalar machEps = btScalar(1.);
33  if (!calculated)
34  {
35  do
36  {
37  machEps /= btScalar(2.0);
38  // If next epsilon yields 1, then break, because current
39  // epsilon is the machine epsilon.
40  } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
41  // printf( "\nCalculated Machine epsilon: %G\n", machEps );
42  calculated = true;
43  }
44  return machEps;
45 }
46 
48 {
49  static btScalar epsroot = 0.;
50  static bool alreadyCalculated = false;
51 
52  if (!alreadyCalculated)
53  {
54  epsroot = btSqrt(btMachEps());
55  alreadyCalculated = true;
56  }
57  return epsroot;
58 }
59 
60 btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
61 {
62  steps = 0;
63 
64  int dim = m_q.size();
65 #ifdef BT_DEBUG_OSTREAM
66  if (DEBUGLEVEL >= 1)
67  {
68  cout << "Dimension = " << dim << endl;
69  }
70 #endif //BT_DEBUG_OSTREAM
71 
72  btVectorXu solutionVector(2 * dim);
73  solutionVector.setZero();
74 
75  //, INIT, 0.);
76 
77  btMatrixXu ident(dim, dim);
78  ident.setIdentity();
79 #ifdef BT_DEBUG_OSTREAM
80  cout << m_M << std::endl;
81 #endif
82 
83  btMatrixXu mNeg = m_M.negative();
84 
85  btMatrixXu A(dim, 2 * dim + 2);
86  //
87  A.setSubMatrix(0, 0, dim - 1, dim - 1, ident);
88  A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg);
89  A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
90  A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1, m_q);
91 
92 #ifdef BT_DEBUG_OSTREAM
93  cout << A << std::endl;
94 #endif //BT_DEBUG_OSTREAM
95 
96  // btVectorXu q_;
97  // q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
98 
100  //At first, all w-values are in the basis
101  for (int i = 0; i < dim; i++)
102  basis.push_back(i);
103 
104  int pivotRowIndex = -1;
105  btScalar minValue = 1e30f;
106  bool greaterZero = true;
107  for (int i = 0; i < dim; i++)
108  {
109  btScalar v = A(i, 2 * dim + 1);
110  if (v < minValue)
111  {
112  minValue = v;
113  pivotRowIndex = i;
114  }
115  if (v < 0)
116  greaterZero = false;
117  }
118 
119  // int pivotRowIndex = q_.minIndex();//minIndex(q_); // first row is that with lowest q-value
120  int z0Row = pivotRowIndex; // remember the col of z0 for ending algorithm afterwards
121  int pivotColIndex = 2 * dim; // first col is that of z0
122 
123 #ifdef BT_DEBUG_OSTREAM
124  if (DEBUGLEVEL >= 3)
125  {
126  // cout << "A: " << A << endl;
127  cout << "pivotRowIndex " << pivotRowIndex << endl;
128  cout << "pivotColIndex " << pivotColIndex << endl;
129  cout << "Basis: ";
130  for (int i = 0; i < basis.size(); i++)
131  cout << basis[i] << " ";
132  cout << endl;
133  }
134 #endif //BT_DEBUG_OSTREAM
135 
136  if (!greaterZero)
137  {
138  if (maxloops == 0)
139  {
140  maxloops = 100;
141  // maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
142  }
143 
144  /*start looping*/
145  for (steps = 0; steps < maxloops; steps++)
146  {
147  GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
148 #ifdef BT_DEBUG_OSTREAM
149  if (DEBUGLEVEL >= 3)
150  {
151  // cout << "A: " << A << endl;
152  cout << "pivotRowIndex " << pivotRowIndex << endl;
153  cout << "pivotColIndex " << pivotColIndex << endl;
154  cout << "Basis: ";
155  for (int i = 0; i < basis.size(); i++)
156  cout << basis[i] << " ";
157  cout << endl;
158  }
159 #endif //BT_DEBUG_OSTREAM
160 
161  int pivotColIndexOld = pivotColIndex;
162 
163  /*find new column index */
164  if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
165  pivotColIndex = basis[pivotRowIndex] + dim;
166  else
167  //else do it the other way round and get in the corresponding w-value
168  pivotColIndex = basis[pivotRowIndex] - dim;
169 
170  /*the column becomes part of the basis*/
171  basis[pivotRowIndex] = pivotColIndexOld;
172 
173  pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
174 
175  if (z0Row == pivotRowIndex)
176  { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
177  GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
178  basis[pivotRowIndex] = pivotColIndex; //update basis
179  break;
180  }
181  }
182 #ifdef BT_DEBUG_OSTREAM
183  if (DEBUGLEVEL >= 1)
184  {
185  cout << "Number of loops: " << steps << endl;
186  cout << "Number of maximal loops: " << maxloops << endl;
187  }
188 #endif //BT_DEBUG_OSTREAM
189 
190  if (!validBasis(basis))
191  {
192  info = -1;
193 #ifdef BT_DEBUG_OSTREAM
194  if (DEBUGLEVEL >= 1)
195  cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
196 #endif //BT_DEBUG_OSTREAM
197 
198  return solutionVector;
199  }
200  }
201 #ifdef BT_DEBUG_OSTREAM
202  if (DEBUGLEVEL >= 2)
203  {
204  // cout << "A: " << A << endl;
205  cout << "pivotRowIndex " << pivotRowIndex << endl;
206  cout << "pivotColIndex " << pivotColIndex << endl;
207  }
208 #endif //BT_DEBUG_OSTREAM
209 
210  for (int i = 0; i < basis.size(); i++)
211  {
212  solutionVector[basis[i]] = A(i, 2 * dim + 1); //q_[i];
213  }
214 
215  info = 0;
216 
217  return solutionVector;
218 }
219 
220 int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
221 {
222  int RowIndex = 0;
223  int dim = A.rows();
225  for (int row = 0; row < dim; row++)
226  {
227  btVectorXu vec(dim + 1);
228  vec.setZero(); //, INIT, 0.)
229  Rows.push_back(vec);
230  btScalar a = A(row, pivotColIndex);
231  if (a > 0)
232  {
233  Rows[row][0] = A(row, 2 * dim + 1) / a;
234  Rows[row][1] = A(row, 2 * dim) / a;
235  for (int j = 2; j < dim + 1; j++)
236  Rows[row][j] = A(row, j - 1) / a;
237 
238 #ifdef BT_DEBUG_OSTREAM
239  // if (DEBUGLEVEL) {
240  // cout << "Rows(" << row << ") = " << Rows[row] << endl;
241  // }
242 #endif
243  }
244  }
245 
246  for (int i = 0; i < Rows.size(); i++)
247  {
248  if (Rows[i].nrm2() > 0.)
249  {
250  int j = 0;
251  for (; j < Rows.size(); j++)
252  {
253  if (i != j)
254  {
255  if (Rows[j].nrm2() > 0.)
256  {
257  btVectorXu test(dim + 1);
258  for (int ii = 0; ii < dim + 1; ii++)
259  {
260  test[ii] = Rows[j][ii] - Rows[i][ii];
261  }
262 
263  //=Rows[j] - Rows[i]
264  if (!LexicographicPositive(test))
265  break;
266  }
267  }
268  }
269 
270  if (j == Rows.size())
271  {
272  RowIndex += i;
273  break;
274  }
275  }
276  }
277 
278  return RowIndex;
279 }
280 
282 {
283  int i = 0;
284  // if (DEBUGLEVEL)
285  // cout << "v " << v << endl;
286 
287  while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
288  i++;
289  if (v[i] > 0)
290  return true;
291 
292  return false;
293 }
294 
295 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
296 {
297  btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
298 #ifdef BT_DEBUG_OSTREAM
299  cout << A << std::endl;
300 #endif
301 
302  for (int i = 0; i < A.rows(); i++)
303  {
304  if (i != pivotRowIndex)
305  {
306  for (int j = 0; j < A.cols(); j++)
307  {
308  if (j != pivotColumnIndex)
309  {
310  btScalar v = A(i, j);
311  v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
312  A.setElem(i, j, v);
313  }
314  }
315  }
316  }
317 
318 #ifdef BT_DEBUG_OSTREAM
319  cout << A << std::endl;
320 #endif //BT_DEBUG_OSTREAM
321  for (int i = 0; i < A.cols(); i++)
322  {
323  A.mulElem(pivotRowIndex, i, -a);
324  }
325 #ifdef BT_DEBUG_OSTREAM
326  cout << A << std::endl;
327 #endif //#ifdef BT_DEBUG_OSTREAM
328 
329  for (int i = 0; i < A.rows(); i++)
330  {
331  if (i != pivotRowIndex)
332  {
333  A.setElem(i, pivotColumnIndex, 0);
334  }
335  }
336 #ifdef BT_DEBUG_OSTREAM
337  cout << A << std::endl;
338 #endif //#ifdef BT_DEBUG_OSTREAM
339 }
340 
342 {
343  bool isGreater = true;
344  for (int i = 0; i < vector.size(); i++)
345  {
346  if (vector[i] < 0)
347  {
348  isGreater = false;
349  break;
350  }
351  }
352 
353  return isGreater;
354 }
355 
357 {
358  bool isValid = true;
359  for (int i = 0; i < basis.size(); i++)
360  {
361  if (basis[i] >= basis.size() * 2)
362  { //then z0 is in the base
363  isValid = false;
364  break;
365  }
366  }
367 
368  return isValid;
369 }
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a vector
ATTR_WARN_UNUSED_RESULT const BMVert * v
#define A
btScalar btMachEps()
btScalar btEpsRoot()
#define btMatrixXu
Definition: btMatrixX.h:529
#define btVectorXu
Definition: btMatrixX.h:528
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void push_back(const T &_Val)
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex)
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simula...
bool validBasis(const btAlignedObjectArray< int > &basis)
bool greaterZero(const btVectorXu &vector)
bool LexicographicPositive(const btVectorXu &v)
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)
ccl_device_inline float2 fabs(const float2 &a)
Definition: math_float2.h:222
static unsigned a[3]
Definition: RandGen.cpp:78
static const int steps
Definition: sky_nishita.cpp:19
float size
Definition: particles.h:27