2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / lcp.cpp
1 /*************************************************************************
2  *                                                                       *
3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
5  *                                                                       *
6  * This library is free software; you can redistribute it and/or         *
7  * modify it under the terms of EITHER:                                  *
8  *   (1) The GNU Lesser General Public License as published by the Free  *
9  *       Software Foundation; either version 2.1 of the License, or (at  *
10  *       your option) any later version. The text of the GNU Lesser      *
11  *       General Public License is included with this library in the     *
12  *       file LICENSE.TXT.                                               *
13  *   (2) The BSD-style license that is included with this library in     *
14  *       the file LICENSE-BSD.TXT.                                       *
15  *                                                                       *
16  * This library is distributed in the hope that it will be useful,       *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
20  *                                                                       *
21  *************************************************************************/
22
23 /*
24
25
26 THE ALGORITHM
27 -------------
28
29 solve A*x = b+w, with x and w subject to certain LCP conditions.
30 each x(i),w(i) must lie on one of the three line segments in the following
31 diagram. each line segment corresponds to one index set :
32
33      w(i)
34      /|\      |           :
35       |       |           :
36       |       |i in N     :
37   w>0 |       |state[i]=0 :
38       |       |           :
39       |       |           :  i in C
40   w=0 +       +-----------------------+
41       |                   :           |
42       |                   :           |
43   w<0 |                   :           |i in N
44       |                   :           |state[i]=1
45       |                   :           |
46       |                   :           |
47       +-------|-----------|-----------|----------> x(i)
48              lo           0           hi
49
50 the Dantzig algorithm proceeds as follows:
51   for i=1:n
52     * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or
53       negative towards the line. as this is done, the other (x(j),w(j))
54       for j<i are constrained to be on the line. if any (x,w) reaches the
55       end of a line segment then it is switched between index sets.
56     * i is added to the appropriate index set depending on what line segment
57       it hits.
58
59 we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit
60 simpler, because the starting point for x(i),w(i) is always on the dotted
61 line x=0 and x will only ever increase in one direction, so it can only hit
62 two out of the three line segments.
63
64
65 NOTES
66 -----
67
68 this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".
69 the implementation is split into an LCP problem object (dLCP) and an LCP
70 driver function. most optimization occurs in the dLCP object.
71
72 a naive implementation of the algorithm requires either a lot of data motion
73 or a lot of permutation-array lookup, because we are constantly re-ordering
74 rows and columns. to avoid this and make a more optimized algorithm, a
75 non-trivial data structure is used to represent the matrix A (this is
76 implemented in the fast version of the dLCP object).
77
78 during execution of this algorithm, some indexes in A are clamped (set C),
79 some are non-clamped (set N), and some are "don't care" (where x=0).
80 A,x,b,w (and other problem vectors) are permuted such that the clamped
81 indexes are first, the unclamped indexes are next, and the don't-care
82 indexes are last. this permutation is recorded in the array `p'.
83 initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,
84 the corresponding elements of p are swapped.
85
86 because the C and N elements are grouped together in the rows of A, we can do
87 lots of work with a fast dot product function. if A,x,etc were not permuted
88 and we only had a permutation array, then those dot products would be much
89 slower as we would have a permutation array lookup in some inner loops.
90
91 A is accessed through an array of row pointers, so that element (i,j) of the
92 permuted matrix is A[i][j]. this makes row swapping fast. for column swapping
93 we still have to actually move the data.
94
95 during execution of this algorithm we maintain an L*D*L' factorization of
96 the clamped submatrix of A (call it `AC') which is the top left nC*nC
97 submatrix of A. there are two ways we could arrange the rows/columns in AC.
98
99 (1) AC is always permuted such that L*D*L' = AC. this causes a problem
100     when a row/column is removed from C, because then all the rows/columns of A
101     between the deleted index and the end of C need to be rotated downward.
102     this results in a lot of data motion and slows things down.
103 (2) L*D*L' is actually a factorization of a *permutation* of AC (which is
104     itself a permutation of the underlying A). this is what we do - the
105     permutation is recorded in the vector C. call this permutation A[C,C].
106     when a row/column is removed from C, all we have to do is swap two
107     rows/columns and manipulate C.
108
109 */
110
111 #include <ode/common.h>
112 #include "lcp.h"
113 #include <ode/matrix.h>
114 #include <ode/misc.h>
115 #include "mat.h"                // for testing
116 #include <ode/timer.h>          // for testing
117
118 //***************************************************************************
119 // code generation parameters
120
121 // LCP debugging (mosty for fast dLCP) - this slows things down a lot
122 //#define DEBUG_LCP
123
124 //#define dLCP_SLOW             // use slow dLCP object
125 #define dLCP_FAST               // use fast dLCP object
126
127 // option 1 : matrix row pointers (less data copying)
128 #define ROWPTRS
129 #define ATYPE dReal **
130 #define AROW(i) (A[i])
131
132 // option 2 : no matrix row pointers (slightly faster inner loops)
133 //#define NOROWPTRS
134 //#define ATYPE dReal *
135 //#define AROW(i) (A+(i)*nskip)
136
137 // misc defines
138 #define ALLOCA dALLOCA16
139 //#define dDot myDot
140 #define NUB_OPTIMIZATIONS
141
142 //***************************************************************************
143
144 // an alternative inline dot product, for speed comparisons
145
146 static inline dReal myDot (dReal *a, dReal *b, int n)
147 {
148   dReal sum=0;
149   while (n > 0) {
150     sum += (*a) * (*b);
151     a++;
152     b++;
153     n--;
154   }
155   return sum;
156 }
157
158
159 // swap row/column i1 with i2 in the n*n matrix A. the leading dimension of
160 // A is nskip. this only references and swaps the lower triangle.
161 // if `do_fast_row_swaps' is nonzero and row pointers are being used, then
162 // rows will be swapped by exchanging row pointers. otherwise the data will
163 // be copied.
164
165 static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip,
166                              int do_fast_row_swaps)
167 {
168   int i;
169   dIASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n &&
170             nskip >= n && i1 < i2);
171
172 # ifdef ROWPTRS
173   for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1];
174   for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i];
175   A[i1][i2] = A[i1][i1];
176   A[i1][i1] = A[i2][i1];
177   A[i2][i1] = A[i2][i2];
178   // swap rows, by swapping row pointers
179   if (do_fast_row_swaps) {
180     dReal *tmpp;
181     tmpp = A[i1];
182     A[i1] = A[i2];
183     A[i2] = tmpp;
184   }
185   else {
186     dReal *tmprow = (dReal*) ALLOCA (n * sizeof(dReal));
187     memcpy (tmprow,A[i1],n * sizeof(dReal));
188     memcpy (A[i1],A[i2],n * sizeof(dReal));
189     memcpy (A[i2],tmprow,n * sizeof(dReal));
190   }
191   // swap columns the hard way
192   for (i=i2+1; i<n; i++) {
193     dReal tmp = A[i][i1];
194     A[i][i1] = A[i][i2];
195     A[i][i2] = tmp;
196   }
197 # else
198   dReal tmp,*tmprow = (dReal*) ALLOCA (n * sizeof(dReal));
199   if (i1 > 0) {
200     memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal));
201     memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal));
202     memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal));
203   }
204   for (i=i1+1; i<i2; i++) {
205     tmp = A[i2*nskip+i];
206     A[i2*nskip+i] = A[i*nskip+i1];
207     A[i*nskip+i1] = tmp;
208   }
209   tmp = A[i1*nskip+i1];
210   A[i1*nskip+i1] = A[i2*nskip+i2];
211   A[i2*nskip+i2] = tmp;
212   for (i=i2+1; i<n; i++) {
213     tmp = A[i*nskip+i1];
214     A[i*nskip+i1] = A[i*nskip+i2];
215     A[i*nskip+i2] = tmp;
216   }
217 # endif
218 }
219
220
221 // swap two indexes in the n*n LCP problem. i1 must be <= i2.
222
223 static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo,
224                          dReal *hi, int *p, int *state, int *findex,
225                          int n, int i1, int i2, int nskip,
226                          int do_fast_row_swaps)
227 {
228   dReal tmp;
229   int tmpi;
230   dIASSERT (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n &&
231             i1 <= i2);
232   if (i1==i2) return;
233   swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);
234   tmp = x[i1];
235   x[i1] = x[i2];
236   x[i2] = tmp;
237   tmp = b[i1];
238   b[i1] = b[i2];
239   b[i2] = tmp;
240   tmp = w[i1];
241   w[i1] = w[i2];
242   w[i2] = tmp;
243   tmp = lo[i1];
244   lo[i1] = lo[i2];
245   lo[i2] = tmp;
246   tmp = hi[i1];
247   hi[i1] = hi[i2];
248   hi[i2] = tmp;
249   tmpi = p[i1];
250   p[i1] = p[i2];
251   p[i2] = tmpi;
252   tmpi = state[i1];
253   state[i1] = state[i2];
254   state[i2] = tmpi;
255   if (findex) {
256     tmpi = findex[i1];
257     findex[i1] = findex[i2];
258     findex[i2] = tmpi;
259   }
260 }
261
262
263 // for debugging - check that L,d is the factorization of A[C,C].
264 // A[C,C] has size nC*nC and leading dimension nskip.
265 // L has size nC*nC and leading dimension nskip.
266 // d has size nC.
267
268 #ifdef DEBUG_LCP
269
270 static void checkFactorization (ATYPE A, dReal *_L, dReal *_d,
271                                 int nC, int *C, int nskip)
272 {
273   int i,j;
274   if (nC==0) return;
275
276   // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C]
277   dMatrix A1 (nC,nC);
278   for (i=0; i<nC; i++) {
279     for (j=0; j<=i; j++) A1(i,j) = A1(j,i) = AROW(i)[j];
280   }
281   dMatrix A2 = A1.select (nC,C,nC,C);
282
283   // printf ("A1=\n"); A1.print(); printf ("\n");
284   // printf ("A2=\n"); A2.print(); printf ("\n");
285
286   // compute A3 = L*D*L'
287   dMatrix L (nC,nC,_L,nskip,1);
288   dMatrix D (nC,nC);
289   for (i=0; i<nC; i++) D(i,i) = 1/_d[i];
290   L.clearUpperTriangle();
291   for (i=0; i<nC; i++) L(i,i) = 1;
292   dMatrix A3 = L * D * L.transpose();
293
294   // printf ("L=\n"); L.print(); printf ("\n");
295   // printf ("D=\n"); D.print(); printf ("\n");
296   // printf ("A3=\n"); A2.print(); printf ("\n");
297
298   // compare A2 and A3
299   dReal diff = A2.maxDifference (A3);
300   if (diff > 1e-8)
301     dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff);
302 }
303
304 #endif
305
306
307 // for debugging
308
309 #ifdef DEBUG_LCP
310
311 static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C)
312 {
313   int j,k;
314   dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n);
315   for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i);
316   for (k=i; k<n; k++) dIASSERT (p[k] == k);
317   for (j=0; j<nC; j++) {
318     int C_is_bad = 1;
319     for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0;
320     dIASSERT (C_is_bad==0);
321   }
322 }
323
324 #endif
325
326 //***************************************************************************
327 // dLCP manipulator object. this represents an n*n LCP problem.
328 //
329 // two index sets C and N are kept. each set holds a subset of
330 // the variable indexes 0..n-1. an index can only be in one set.
331 // initially both sets are empty.
332 //
333 // the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
334
335 #ifdef dLCP_SLOW
336
337 // simple but slow implementation of dLCP, for testing the LCP drivers.
338
339 #include "array.h"
340
341 struct dLCP {
342   int n,nub,nskip;
343   dReal *Adata,*x,*b,*w,*lo,*hi;        // LCP problem data
344   ATYPE A;                              // A rows
345   dArray<int> C,N;                      // index sets
346   int last_i_for_solve1;                // last i value given to solve1
347
348   dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
349         dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
350         dReal *_Dell, dReal *_ell, dReal *_tmp,
351         int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
352   // the constructor is given an initial problem description (A,x,b,w) and
353   // space for other working data (which the caller may allocate on the stack).
354   // some of this data is specific to the fast dLCP implementation.
355   // the matrices A and L have size n*n, vectors have size n*1.
356   // A represents a symmetric matrix but only the lower triangle is valid.
357   // `nub' is the number of unbounded indexes at the start. all the indexes
358   // 0..nub-1 will be put into C.
359
360   ~dLCP();
361
362   int getNub() { return nub; }
363   // return the value of `nub'. the constructor may want to change it,
364   // so the caller should find out its new value.
365
366   // transfer functions: transfer index i to the given set (C or N). indexes
367   // less than `nub' can never be given. A,x,b,w,etc may be permuted by these
368   // functions, the caller must be robust to this.
369
370   void transfer_i_to_C (int i);
371     // this assumes C and N span 1:i-1. this also assumes that solve1() has
372     // been recently called for the same i without any other transfer
373     // functions in between (thereby allowing some data reuse for the fast
374     // implementation).
375   void transfer_i_to_N (int i);
376     // this assumes C and N span 1:i-1.
377   void transfer_i_from_N_to_C (int i);
378   void transfer_i_from_C_to_N (int i);
379
380   int numC();
381   int numN();
382   // return the number of indexes in set C/N
383
384   int indexC (int i);
385   int indexN (int i);
386   // return index i in set C/N.
387
388   // accessor and arithmetic functions. Aij translates as A(i,j), etc.
389   // make sure that only the lower triangle of A is ever referenced.
390
391   dReal Aii (int i);
392   dReal AiC_times_qC (int i, dReal *q);
393   dReal AiN_times_qN (int i, dReal *q);                 // for all Nj
394   void pN_equals_ANC_times_qC (dReal *p, dReal *q);     // for all Nj
395   void pN_plusequals_ANi (dReal *p, int i, int sign=1);
396     // for all Nj. sign = +1,-1. assumes i > maximum index in N.
397   void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q);
398   void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj
399   void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
400     // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1.
401     // the fast version of this function computes some data that is needed by
402     // transfer_i_to_C(). if only_transfer is nonzero then this function
403     // *only* computes that data, it does not set a(C).
404
405   void unpermute();
406   // call this at the end of the LCP function. if the x/w values have been
407   // permuted then this will unscramble them.
408 };
409
410
411 dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
412             dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
413             dReal *_Dell, dReal *_ell, dReal *_tmp,
414             int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
415 {
416   dUASSERT (_findex==0,"slow dLCP object does not support findex array");
417
418   n = _n;
419   nub = _nub;
420   Adata = _Adata;
421   A = 0;
422   x = _x;
423   b = _b;
424   w = _w;
425   lo = _lo;
426   hi = _hi;
427   nskip = dPAD(n);
428   dSetZero (x,n);
429   last_i_for_solve1 = -1;
430
431   int i,j;
432   C.setSize (n);
433   N.setSize (n);
434   for (int i=0; i<n; i++) {
435     C[i] = 0;
436     N[i] = 0;
437   }
438
439 # ifdef ROWPTRS
440   // make matrix row pointers
441   A = Arows;
442   for (i=0; i<n; i++) A[i] = Adata + i*nskip;
443 # else
444   A = Adata;
445 # endif
446
447   // lets make A symmetric
448   for (i=0; i<n; i++) {
449     for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
450   }
451
452   // if nub>0, put all indexes 0..nub-1 into C and solve for x
453   if (nub > 0) {
454     for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
455     dFactorLDLT (_L,_d,nub,nskip);
456     memcpy (x,b,nub*sizeof(dReal));
457     dSolveLDLT (_L,_d,x,nub,nskip);
458     dSetZero (_w,nub);
459     for (i=0; i<nub; i++) C[i] = 1;
460   }
461 }
462
463
464 dLCP::~dLCP()
465 {
466 }
467
468
469 void dLCP::transfer_i_to_C (int i)
470 {
471   if (i < nub) dDebug (0,"bad i");
472   if (C[i]) dDebug (0,"i already in C");
473   if (N[i]) dDebug (0,"i already in N");
474   for (int k=0; k<i; k++) {
475     if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated");
476   }
477   for (int k=i; k<n; k++)
478     if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
479   if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated");
480   last_i_for_solve1 = -1;
481   C[i] = 1;
482 }
483
484
485 void dLCP::transfer_i_to_N (int i)
486 {
487   if (i < nub) dDebug (0,"bad i");
488   if (C[i]) dDebug (0,"i already in C");
489   if (N[i]) dDebug (0,"i already in N");
490   for (int k=0; k<i; k++)
491     if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated");
492   for (int k=i; k<n; k++)
493     if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
494   last_i_for_solve1 = -1;
495   N[i] = 1;
496 }
497
498
499 void dLCP::transfer_i_from_N_to_C (int i)
500 {
501   if (i < nub) dDebug (0,"bad i");
502   if (C[i]) dDebug (0,"i already in C");
503   if (!N[i]) dDebug (0,"i not in N");
504   last_i_for_solve1 = -1;
505   N[i] = 0;
506   C[i] = 1;
507 }
508
509
510 void dLCP::transfer_i_from_C_to_N (int i)
511 {
512   if (i < nub) dDebug (0,"bad i");
513   if (N[i]) dDebug (0,"i already in N");
514   if (!C[i]) dDebug (0,"i not in C");
515   last_i_for_solve1 = -1;
516   C[i] = 0;
517   N[i] = 1;
518 }
519
520
521 int dLCP::numC()
522 {
523   int i,count=0;
524   for (i=0; i<n; i++) if (C[i]) count++;
525   return count;
526 }
527
528
529 int dLCP::numN()
530 {
531   int i,count=0;
532   for (i=0; i<n; i++) if (N[i]) count++;
533   return count;
534 }
535
536
537 int dLCP::indexC (int i)
538 {
539   int k,count=0;
540   for (k=0; k<n; k++) {
541     if (C[k]) {
542       if (count==i) return k;
543       count++;
544     }
545   }
546   dDebug (0,"bad index C (%d)",i);
547   return 0;
548 }
549
550
551 int dLCP::indexN (int i)
552 {
553   int k,count=0;
554   for (k=0; k<n; k++) {
555     if (N[k]) {
556       if (count==i) return k;
557       count++;
558     }
559   }
560   dDebug (0,"bad index into N");
561   return 0;
562 }
563
564
565 dReal dLCP::Aii (int i)
566 {
567   return AROW(i)[i];
568 }
569
570
571 dReal dLCP::AiC_times_qC (int i, dReal *q)
572 {
573   dReal sum = 0;
574   for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k];
575   return sum;
576 }
577
578
579 dReal dLCP::AiN_times_qN (int i, dReal *q)
580 {
581   dReal sum = 0;
582   for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k];
583   return sum;
584 }
585
586
587 void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
588 {
589   dReal sum;
590   for (int ii=0; ii<n; ii++) if (N[ii]) {
591     sum = 0;
592     for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj];
593     p[ii] = sum;
594   }
595 }
596
597
598 void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
599 {
600   int k;
601   for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated");
602   if (sign > 0) {
603     for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k];
604   }
605   else {
606     for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k];
607   }
608 }
609
610
611 void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
612 {
613   for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k];
614 }
615
616
617 void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
618 {
619   for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k];
620 }
621
622
623 void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
624 {
625   dReal *AA = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
626   dReal *dd = (dReal*) ALLOCA (n*sizeof(dReal));
627   dReal *bb = (dReal*) ALLOCA (n*sizeof(dReal));
628   int ii,jj,AAi,AAj;
629
630   last_i_for_solve1 = i;
631   AAi = 0;
632   for (ii=0; ii<n; ii++) if (C[ii]) {
633     AAj = 0;
634     for (jj=0; jj<n; jj++) if (C[jj]) {
635       AA[AAi*nskip+AAj] = AROW(ii)[jj];
636       AAj++;
637     }
638     bb[AAi] = AROW(i)[ii];
639     AAi++;
640   }
641   if (AAi==0) return;
642
643   dFactorLDLT (AA,dd,AAi,nskip);
644   dSolveLDLT (AA,dd,bb,AAi,nskip);
645
646   AAi=0;
647   if (dir > 0) {
648     for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
649   }
650   else {
651     for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
652   }
653 }
654
655
656 void dLCP::unpermute()
657 {
658 }
659
660 #endif // dLCP_SLOW
661
662 //***************************************************************************
663 // fast implementation of dLCP. see the above definition of dLCP for
664 // interface comments.
665 //
666 // `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is
667 // permuted as the other vectors/matrices are permuted.
668 //
669 // A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have
670 // contiguous indexes. the don't-care indexes follow N.
671 //
672 // an L*D*L' factorization is maintained of A(C,C), and whenever indexes are
673 // added or removed from the set C the factorization is updated.
674 // thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.
675 // the leading dimension of the matrix L is always `nskip'.
676 //
677 // at the start there may be other indexes that are unbounded but are not
678 // included in `nub'. dLCP will permute the matrix so that absolutely all
679 // unbounded vectors are at the start. thus there may be some initial
680 // permutation.
681 //
682 // the algorithms here assume certain patterns, particularly with respect to
683 // index transfer.
684
685 #ifdef dLCP_FAST
686
687 struct dLCP {
688   int n,nskip,nub;
689   ATYPE A;                              // A rows
690   dReal *Adata,*x,*b,*w,*lo,*hi;        // permuted LCP problem data
691   dReal *L,*d;                          // L*D*L' factorization of set C
692   dReal *Dell,*ell,*tmp;
693   int *state,*findex,*p,*C;
694   int nC,nN;                            // size of each index set
695
696   dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
697         dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
698         dReal *_Dell, dReal *_ell, dReal *_tmp,
699         int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
700   int getNub() { return nub; }
701   void transfer_i_to_C (int i);
702   void transfer_i_to_N (int i)
703     { nN++; }                   // because we can assume C and N span 1:i-1
704   void transfer_i_from_N_to_C (int i);
705   void transfer_i_from_C_to_N (int i);
706   int numC() { return nC; }
707   int numN() { return nN; }
708   int indexC (int i) { return i; }
709   int indexN (int i) { return i+nC; }
710   dReal Aii (int i) { return AROW(i)[i]; }
711   dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); }
712   dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); }
713   void pN_equals_ANC_times_qC (dReal *p, dReal *q);
714   void pN_plusequals_ANi (dReal *p, int i, int sign=1);
715   void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
716     { for (int i=0; i<nC; i++) p[i] += s*q[i]; }
717   void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
718     { for (int i=0; i<nN; i++) p[i+nC] += s*q[i+nC]; }
719   void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
720   void unpermute();
721 };
722
723
724 dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
725             dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
726             dReal *_Dell, dReal *_ell, dReal *_tmp,
727             int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
728 {
729   n = _n;
730   nub = _nub;
731   Adata = _Adata;
732   A = 0;
733   x = _x;
734   b = _b;
735   w = _w;
736   lo = _lo;
737   hi = _hi;
738   L = _L;
739   d = _d;
740   Dell = _Dell;
741   ell = _ell;
742   tmp = _tmp;
743   state = _state;
744   findex = _findex;
745   p = _p;
746   C = _C;
747   nskip = dPAD(n);
748   dSetZero (x,n);
749
750   int k;
751
752 # ifdef ROWPTRS
753   // make matrix row pointers
754   A = Arows;
755   for (k=0; k<n; k++) A[k] = Adata + k*nskip;
756 # else
757   A = Adata;
758 # endif
759
760   nC = 0;
761   nN = 0;
762   for (k=0; k<n; k++) p[k]=k;           // initially unpermuted
763
764   /*
765   // for testing, we can do some random swaps in the area i > nub
766   if (nub < n) {
767     for (k=0; k<100; k++) {
768       int i1,i2;
769       do {
770         i1 = dRandInt(n-nub)+nub;
771         i2 = dRandInt(n-nub)+nub;
772       }
773       while (i1 > i2); 
774       //printf ("--> %d %d\n",i1,i2);
775       swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0);
776     }
777   }
778   */
779
780   // permute the problem so that *all* the unbounded variables are at the
781   // start, i.e. look for unbounded variables not included in `nub'. we can
782   // potentially push up `nub' this way and get a bigger initial factorization.
783   // note that when we swap rows/cols here we must not just swap row pointers,
784   // as the initial factorization relies on the data being all in one chunk.
785   for (k=nub; k<n; k++) {
786     if (lo[k]==-dInfinity && hi[k]==dInfinity) {
787       swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);
788       nub++;
789     }
790   }
791
792   // if there are unbounded variables at the start, factorize A up to that
793   // point and solve for x. this puts all indexes 0..nub-1 into C.
794   if (nub > 0) {
795     for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));
796     dFactorLDLT (L,d,nub,nskip);
797     memcpy (x,b,nub*sizeof(dReal));
798     dSolveLDLT (L,d,x,nub,nskip);
799     dSetZero (w,nub);
800     for (k=0; k<nub; k++) C[k] = k;
801     nC = nub;
802   }
803
804   // permute the indexes > nub such that all findex variables are at the end
805   if (findex) {
806     int num_at_end = 0;
807     for (k=n-1; k >= nub; k--) {
808       if (findex[k] >= 0) {
809         swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);
810         num_at_end++;
811       }
812     }
813   }
814
815   // print info about indexes
816   /*
817   for (k=0; k<n; k++) {
818     if (k<nub) printf ("C");
819     else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");
820     else printf (".");
821   }
822   printf ("\n");
823   */
824 }
825
826
827 void dLCP::transfer_i_to_C (int i)
828 {
829   int j;
830   if (nC > 0) {
831     // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))
832     for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
833     d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
834   }
835   else {
836     d[0] = dRecip (AROW(i)[i]);
837   }
838   swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
839   C[nC] = nC;
840   nC++;
841
842 # ifdef DEBUG_LCP
843   checkFactorization (A,L,d,nC,C,nskip);
844   if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C);
845 # endif
846 }
847
848
849 void dLCP::transfer_i_from_N_to_C (int i)
850 {
851   int j;
852   if (nC > 0) {
853     dReal *aptr = AROW(i);
854 #   ifdef NUB_OPTIMIZATIONS
855     // if nub>0, initial part of aptr unpermuted
856     for (j=0; j<nub; j++) Dell[j] = aptr[j];
857     for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
858 #   else
859     for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
860 #   endif
861     dSolveL1 (L,Dell,nC,nskip);
862     for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
863     for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];
864     d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));
865   }
866   else {
867     d[0] = dRecip (AROW(i)[i]);
868   }
869   swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);
870   C[nC] = nC;
871   nN--;
872   nC++;
873
874   // @@@ TO DO LATER
875   // if we just finish here then we'll go back and re-solve for
876   // delta_x. but actually we can be more efficient and incrementally
877   // update delta_x here. but if we do this, we wont have ell and Dell
878   // to use in updating the factorization later.
879
880 # ifdef DEBUG_LCP
881   checkFactorization (A,L,d,nC,C,nskip);
882 # endif
883 }
884
885
886 void dLCP::transfer_i_from_C_to_N (int i)
887 {
888   // remove a row/column from the factorization, and adjust the
889   // indexes (black magic!)
890   int j,k;
891   for (j=0; j<nC; j++) if (C[j]==i) {
892     dLDLTRemove (A,C,L,d,n,nC,j,nskip);
893     for (k=0; k<nC; k++) if (C[k]==nC-1) {
894       C[k] = C[j];
895       if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));
896       break;
897     }
898     dIASSERT (k < nC);
899     break;
900   }
901   dIASSERT (j < nC);
902   swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);
903   nC--;
904   nN++;
905
906 # ifdef DEBUG_LCP
907   checkFactorization (A,L,d,nC,C,nskip);
908 # endif
909 }
910
911
912 void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
913 {
914   // we could try to make this matrix-vector multiplication faster using
915   // outer product matrix tricks, e.g. with the dMultidotX() functions.
916   // but i tried it and it actually made things slower on random 100x100
917   // problems because of the overhead involved. so we'll stick with the
918   // simple method for now.
919   for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC);
920 }
921
922
923 void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
924 {
925   dReal *aptr = AROW(i)+nC;
926   if (sign > 0) {
927     for (int i=0; i<nN; i++) p[i+nC] += aptr[i];
928   }
929   else {
930     for (int i=0; i<nN; i++) p[i+nC] -= aptr[i];
931   }
932 }
933
934
935 void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
936 {
937   // the `Dell' and `ell' that are computed here are saved. if index i is
938   // later added to the factorization then they can be reused.
939   //
940   // @@@ question: do we need to solve for entire delta_x??? yes, but
941   //     only if an x goes below 0 during the step.
942
943   int j;
944   if (nC > 0) {
945     dReal *aptr = AROW(i);
946 #   ifdef NUB_OPTIMIZATIONS
947     // if nub>0, initial part of aptr[] is guaranteed unpermuted
948     for (j=0; j<nub; j++) Dell[j] = aptr[j];
949     for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];
950 #   else
951     for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];
952 #   endif
953     dSolveL1 (L,Dell,nC,nskip);
954     for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j];
955
956     if (!only_transfer) {
957       for (j=0; j<nC; j++) tmp[j] = ell[j];
958       dSolveL1T (L,tmp,nC,nskip);
959       if (dir > 0) {
960         for (j=0; j<nC; j++) a[C[j]] = -tmp[j];
961       }
962       else {
963         for (j=0; j<nC; j++) a[C[j]] = tmp[j];
964       }
965     }
966   }
967 }
968
969
970 void dLCP::unpermute()
971 {
972   // now we have to un-permute x and w
973   int j;
974   dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
975   memcpy (tmp,x,n*sizeof(dReal));
976   for (j=0; j<n; j++) x[p[j]] = tmp[j];
977   memcpy (tmp,w,n*sizeof(dReal));
978   for (j=0; j<n; j++) w[p[j]] = tmp[j];
979 }
980
981 #endif // dLCP_FAST
982
983 //***************************************************************************
984 // an unoptimized Dantzig LCP driver routine for the basic LCP problem.
985 // must have lo=0, hi=dInfinity, and nub=0.
986
987 void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
988                      dReal *w, int nub, dReal *lo, dReal *hi)
989 {
990   dAASSERT (n>0 && A && x && b && w && nub == 0);
991
992   int i,k;
993   int nskip = dPAD(n);
994   dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
995   dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
996   dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
997   dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
998   dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
999   dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
1000   dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
1001   dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
1002   int *p = (int*) ALLOCA (n*sizeof(int));
1003   int *C = (int*) ALLOCA (n*sizeof(int));
1004   int *dummy = (int*) ALLOCA (n*sizeof(int));
1005
1006   dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
1007   nub = lcp.getNub();
1008
1009   for (i=0; i<n; i++) {
1010     w[i] = lcp.AiC_times_qC (i,x) - b[i];
1011     if (w[i] >= 0) {
1012       lcp.transfer_i_to_N (i);
1013     }
1014     else {
1015       for (;;) {
1016         // compute: delta_x(C) = -A(C,C)\A(C,i)
1017         dSetZero (delta_x,n);
1018         lcp.solve1 (delta_x,i);
1019         delta_x[i] = 1;
1020
1021         // compute: delta_w = A*delta_x
1022         dSetZero (delta_w,n);
1023         lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
1024         lcp.pN_plusequals_ANi (delta_w,i);
1025         delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
1026
1027         // find index to switch
1028         int si = i;             // si = switch index
1029         int si_in_N = 0;        // set to 1 if si in N
1030         dReal s = -w[i]/delta_w[i];
1031
1032         if (s <= 0) {
1033           dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
1034           if (i < (n-1)) {
1035             dSetZero (x+i,n-i);
1036             dSetZero (w+i,n-i);
1037           }
1038           goto done;
1039         }
1040
1041         for (k=0; k < lcp.numN(); k++) {
1042           if (delta_w[lcp.indexN(k)] < 0) {
1043             dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
1044             if (s2 < s) {
1045               s = s2;
1046               si = lcp.indexN(k);
1047               si_in_N = 1;
1048             }
1049           }
1050         }
1051         for (k=0; k < lcp.numC(); k++) {
1052           if (delta_x[lcp.indexC(k)] < 0) {
1053             dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
1054             if (s2 < s) {
1055               s = s2;
1056               si = lcp.indexC(k);
1057               si_in_N = 0;
1058             }
1059           }
1060         }
1061
1062         // apply x = x + s * delta_x
1063         lcp.pC_plusequals_s_times_qC (x,s,delta_x);
1064         x[i] += s;
1065         lcp.pN_plusequals_s_times_qN (w,s,delta_w);
1066         w[i] += s * delta_w[i];
1067
1068         // switch indexes between sets if necessary
1069         if (si==i) {
1070           w[i] = 0;
1071           lcp.transfer_i_to_C (i);
1072           break;
1073         }
1074         if (si_in_N) {
1075           w[si] = 0;
1076           lcp.transfer_i_from_N_to_C (si);
1077         }
1078         else {
1079           x[si] = 0;
1080           lcp.transfer_i_from_C_to_N (si);
1081         }
1082       }
1083     }
1084   }
1085
1086  done:
1087   lcp.unpermute();
1088 }
1089
1090 //***************************************************************************
1091 // an optimized Dantzig LCP driver routine for the lo-hi LCP problem.
1092
1093 void dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
1094                 dReal *w, int nub, dReal *lo, dReal *hi, int *findex)
1095 {
1096   dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n);
1097   int i,k,hit_first_friction_index = 0;
1098   int nskip = dPAD(n);
1099
1100   // if all the variables are unbounded then we can just factor, solve,
1101   // and return
1102   if (nub >= n) {
1103     dFactorLDLT (A,w,n,nskip);          // use w for d
1104     dSolveLDLT (A,w,b,n,nskip);
1105     memcpy (x,b,n*sizeof(dReal));
1106     dSetZero (w,n);
1107     return;
1108   }
1109
1110 # ifndef dNODEBUG
1111   // check restrictions on lo and hi
1112   for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
1113 # endif
1114
1115   dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
1116   dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
1117   dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
1118   dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
1119   dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
1120   dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
1121   dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
1122   int *p = (int*) ALLOCA (n*sizeof(int));
1123   int *C = (int*) ALLOCA (n*sizeof(int));
1124   int dir;
1125   dReal dirf;
1126
1127   // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
1128   int *state = (int*) ALLOCA (n*sizeof(int));
1129
1130   // create LCP object. note that tmp is set to delta_w to save space, this
1131   // optimization relies on knowledge of how tmp is used, so be careful!
1132   dLCP lcp (n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
1133   nub = lcp.getNub();
1134
1135   // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the
1136   // LCP conditions then i is added to the appropriate index set. otherwise
1137   // x(i),w(i) is driven either +ve or -ve to force it to the valid region.
1138   // as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
1139   // while driving x(i) we maintain the LCP conditions on the other variables
1140   // 0..i-1. we do this by watching out for other x(i),w(i) values going
1141   // outside the valid region, and then switching them between index sets
1142   // when that happens.
1143
1144   for (i=nub; i<n; i++) {
1145     // the index i is the driving index and indexes i+1..n-1 are "dont care",
1146     // i.e. when we make changes to the system those x's will be zero and we
1147     // don't care what happens to those w's. in other words, we only consider
1148     // an (i+1)*(i+1) sub-problem of A*x=b+w.
1149
1150     // if we've hit the first friction index, we have to compute the lo and
1151     // hi values based on the values of x already computed. we have been
1152     // permuting the indexes, so the values stored in the findex vector are
1153     // no longer valid. thus we have to temporarily unpermute the x vector. 
1154     if (hit_first_friction_index == 0 && findex && findex[i] >= 0) {
1155       // un-permute x into delta_w, which is not being used at the moment
1156       for (k=0; k<n; k++) delta_w[p[k]] = x[k];
1157       // set lo and hi values
1158       for (k=i; k<n; k++) {
1159         hi[k] = dFabs (hi[k] * delta_w[findex[k]]);
1160         lo[k] = -hi[k];
1161       }
1162       hit_first_friction_index = 1;
1163     }
1164
1165     // thus far we have not even been computing the w values for indexes
1166     // greater than i, so compute w[i] now.
1167     w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
1168
1169     // if lo=hi=0 (which can happen for tangential friction when normals are
1170     // 0) then the index will be assigned to set N with some state. however,
1171     // set C's line has zero size, so the index will always remain in set N.
1172     // with the "normal" switching logic, if w changed sign then the index
1173     // would have to switch to set C and then back to set N with an inverted
1174     // state. this is pointless, and also computationally expensive. to
1175     // prevent this from happening, we use the rule that indexes with lo=hi=0
1176     // will never be checked for set changes. this means that the state for
1177     // these indexes may be incorrect, but that doesn't matter.
1178
1179     // see if x(i),w(i) is in a valid region
1180     if (lo[i]==0 && w[i] >= 0) {
1181       lcp.transfer_i_to_N (i);
1182       state[i] = 0;
1183     }
1184     else if (hi[i]==0 && w[i] <= 0) {
1185       lcp.transfer_i_to_N (i);
1186       state[i] = 1;
1187     }
1188     else if (w[i]==0) {
1189       // this is a degenerate case. by the time we get to this test we know
1190       // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
1191       // and similarly that hi > 0. this means that the line segment
1192       // corresponding to set C is at least finite in extent, and we are on it.
1193       // NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
1194       lcp.solve1 (delta_x,i,0,1);
1195       lcp.transfer_i_to_C (i);
1196     }
1197     else {
1198       // we must push x(i) and w(i)
1199       for (;;) {
1200         // find direction to push on x(i)
1201         if (w[i] <= 0) {
1202           dir = 1;
1203           dirf = REAL(1.0);
1204         }
1205         else {
1206           dir = -1;
1207           dirf = REAL(-1.0);
1208         }
1209
1210         // compute: delta_x(C) = -dir*A(C,C)\A(C,i)
1211         lcp.solve1 (delta_x,i,dir);
1212         // note that delta_x[i] = dirf, but we wont bother to set it
1213
1214         // compute: delta_w = A*delta_x ... note we only care about
1215         // delta_w(N) and delta_w(i), the rest is ignored
1216         lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
1217         lcp.pN_plusequals_ANi (delta_w,i,dir);
1218         delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
1219
1220         // find largest step we can take (size=s), either to drive x(i),w(i)
1221         // to the valid LCP region or to drive an already-valid variable
1222         // outside the valid region.
1223
1224         int cmd = 1;            // index switching command
1225         int si = 0;             // si = index to switch if cmd>3
1226         dReal s = -w[i]/delta_w[i];
1227         if (dir > 0) {
1228           if (hi[i] < dInfinity) {
1229             dReal s2 = (hi[i]-x[i])/dirf;               // step to x(i)=hi(i)
1230             if (s2 < s) {
1231               s = s2;
1232               cmd = 3;
1233             }
1234           }
1235         }
1236         else {
1237           if (lo[i] > -dInfinity) {
1238             dReal s2 = (lo[i]-x[i])/dirf;               // step to x(i)=lo(i)
1239             if (s2 < s) {
1240               s = s2;
1241               cmd = 2;
1242             }
1243           }
1244         }
1245
1246         for (k=0; k < lcp.numN(); k++) {
1247           if ((state[lcp.indexN(k)]==0 && delta_w[lcp.indexN(k)] < 0) ||
1248               (state[lcp.indexN(k)]!=0 && delta_w[lcp.indexN(k)] > 0)) {
1249             // don't bother checking if lo=hi=0
1250             if (lo[lcp.indexN(k)] == 0 && hi[lcp.indexN(k)] == 0) continue;
1251             dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
1252             if (s2 < s) {
1253               s = s2;
1254               cmd = 4;
1255               si = lcp.indexN(k);
1256             }
1257           }
1258         }
1259
1260         for (k=nub; k < lcp.numC(); k++) {
1261           if (delta_x[lcp.indexC(k)] < 0 && lo[lcp.indexC(k)] > -dInfinity) {
1262             dReal s2 = (lo[lcp.indexC(k)]-x[lcp.indexC(k)]) /
1263               delta_x[lcp.indexC(k)];
1264             if (s2 < s) {
1265               s = s2;
1266               cmd = 5;
1267               si = lcp.indexC(k);
1268             }
1269           }
1270           if (delta_x[lcp.indexC(k)] > 0 && hi[lcp.indexC(k)] < dInfinity) {
1271             dReal s2 = (hi[lcp.indexC(k)]-x[lcp.indexC(k)]) /
1272               delta_x[lcp.indexC(k)];
1273             if (s2 < s) {
1274               s = s2;
1275               cmd = 6;
1276               si = lcp.indexC(k);
1277             }
1278           }
1279         }
1280
1281         //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
1282         //                           "C->NL","C->NH"};
1283         //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
1284
1285         // if s <= 0 then we've got a problem. if we just keep going then
1286         // we're going to get stuck in an infinite loop. instead, just cross
1287         // our fingers and exit with the current solution.
1288         if (s <= 0) {
1289           dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
1290           if (i < (n-1)) {
1291             dSetZero (x+i,n-i);
1292             dSetZero (w+i,n-i);
1293           }
1294           goto done;
1295         }
1296
1297         // apply x = x + s * delta_x
1298         lcp.pC_plusequals_s_times_qC (x,s,delta_x);
1299         x[i] += s * dirf;
1300
1301         // apply w = w + s * delta_w
1302         lcp.pN_plusequals_s_times_qN (w,s,delta_w);
1303         w[i] += s * delta_w[i];
1304
1305         // switch indexes between sets if necessary
1306         switch (cmd) {
1307         case 1:         // done
1308           w[i] = 0;
1309           lcp.transfer_i_to_C (i);
1310           break;
1311         case 2:         // done
1312           x[i] = lo[i];
1313           state[i] = 0;
1314           lcp.transfer_i_to_N (i);
1315           break;
1316         case 3:         // done
1317           x[i] = hi[i];
1318           state[i] = 1;
1319           lcp.transfer_i_to_N (i);
1320           break;
1321         case 4:         // keep going
1322           w[si] = 0;
1323           lcp.transfer_i_from_N_to_C (si);
1324           break;
1325         case 5:         // keep going
1326           x[si] = lo[si];
1327           state[si] = 0;
1328           lcp.transfer_i_from_C_to_N (si);
1329           break;
1330         case 6:         // keep going
1331           x[si] = hi[si];
1332           state[si] = 1;
1333           lcp.transfer_i_from_C_to_N (si);
1334           break;
1335         }
1336
1337         if (cmd <= 3) break;
1338       }
1339     }
1340   }
1341
1342  done:
1343   lcp.unpermute();
1344 }
1345
1346 //***************************************************************************
1347 // accuracy and timing test
1348
1349 extern "C" void dTestSolveLCP()
1350 {
1351   int n = 100;
1352   int i,nskip = dPAD(n);
1353   const dReal tol = REAL(1e-9);
1354   printf ("dTestSolveLCP()\n");
1355
1356   dReal *A = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
1357   dReal *x = (dReal*) ALLOCA (n*sizeof(dReal));
1358   dReal *b = (dReal*) ALLOCA (n*sizeof(dReal));
1359   dReal *w = (dReal*) ALLOCA (n*sizeof(dReal));
1360   dReal *lo = (dReal*) ALLOCA (n*sizeof(dReal));
1361   dReal *hi = (dReal*) ALLOCA (n*sizeof(dReal));
1362
1363   dReal *A2 = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
1364   dReal *b2 = (dReal*) ALLOCA (n*sizeof(dReal));
1365   dReal *lo2 = (dReal*) ALLOCA (n*sizeof(dReal));
1366   dReal *hi2 = (dReal*) ALLOCA (n*sizeof(dReal));
1367   dReal *tmp1 = (dReal*) ALLOCA (n*sizeof(dReal));
1368   dReal *tmp2 = (dReal*) ALLOCA (n*sizeof(dReal));
1369
1370   double total_time = 0;
1371   for (int count=0; count < 1000; count++) {
1372
1373     // form (A,b) = a random positive definite LCP problem
1374     dMakeRandomMatrix (A2,n,n,1.0);
1375     dMultiply2 (A,A2,A2,n,n,n);
1376     dMakeRandomMatrix (x,n,1,1.0);
1377     dMultiply0 (b,A,x,n,n,1);
1378     for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);
1379
1380     // choose `nub' in the range 0..n-1
1381     int nub = 50; //dRandInt (n);
1382
1383     // make limits
1384     for (i=0; i<nub; i++) lo[i] = -dInfinity;
1385     for (i=0; i<nub; i++) hi[i] = dInfinity;
1386     //for (i=nub; i<n; i++) lo[i] = 0;
1387     //for (i=nub; i<n; i++) hi[i] = dInfinity;
1388     //for (i=nub; i<n; i++) lo[i] = -dInfinity;
1389     //for (i=nub; i<n; i++) hi[i] = 0;
1390     for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
1391     for (i=nub; i<n; i++) hi[i] =  (dRandReal()*REAL(1.0))+REAL(0.01);
1392
1393     // set a few limits to lo=hi=0
1394     /*
1395     for (i=0; i<10; i++) {
1396       int j = dRandInt (n-nub) + nub;
1397       lo[j] = 0;
1398       hi[j] = 0;
1399     }
1400     */
1401
1402     // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
1403     // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
1404     // ensure that it doesn't get referenced (if it does, the answer will be
1405     // wrong).
1406
1407     memcpy (A2,A,n*nskip*sizeof(dReal));
1408     dClearUpperTriangle (A2,n);
1409     memcpy (b2,b,n*sizeof(dReal));
1410     memcpy (lo2,lo,n*sizeof(dReal));
1411     memcpy (hi2,hi,n*sizeof(dReal));
1412     dSetZero (x,n);
1413     dSetZero (w,n);
1414
1415     dStopwatch sw;
1416     dStopwatchReset (&sw);
1417     dStopwatchStart (&sw);
1418
1419     dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);
1420
1421     dStopwatchStop (&sw);
1422     double time = dStopwatchTime(&sw);
1423     total_time += time;
1424     double average = total_time / double(count+1) * 1000.0;
1425
1426     // check the solution
1427
1428     dMultiply0 (tmp1,A,x,n,n,1);
1429     for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
1430     dReal diff = dMaxDifference (tmp1,tmp2,n,1);
1431     // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
1432     //      diff > tol ? "FAILED" : "passed");
1433     if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
1434     int n1=0,n2=0,n3=0;
1435     for (i=0; i<n; i++) {
1436       if (x[i]==lo[i] && w[i] >= 0) {
1437         n1++;   // ok
1438       }
1439       else if (x[i]==hi[i] && w[i] <= 0) {
1440         n2++;   // ok
1441       }
1442       else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
1443         n3++;   // ok
1444       }
1445       else {
1446         dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
1447                 x[i],w[i],lo[i],hi[i]);
1448       }
1449     }
1450
1451     // pacifier
1452     printf ("passed: NL=%3d NH=%3d C=%3d   ",n1,n2,n3);
1453     printf ("time=%10.3f ms  avg=%10.4f\n",time * 1000.0,average);
1454   }
1455 }