[216] | 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 | // use protected, non-stack memory allocation system |
---|
| 138 | |
---|
| 139 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 140 | extern unsigned int dMemoryFlag; |
---|
| 141 | |
---|
| 142 | #define ALLOCA(t,v,s) t* v = (t*) malloc(s) |
---|
| 143 | #define UNALLOCA(t) free(t) |
---|
| 144 | |
---|
| 145 | #else |
---|
| 146 | |
---|
| 147 | #define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s) |
---|
| 148 | #define UNALLOCA(t) /* nothing */ |
---|
| 149 | |
---|
| 150 | #endif |
---|
| 151 | |
---|
| 152 | #define NUB_OPTIMIZATIONS |
---|
| 153 | |
---|
| 154 | //*************************************************************************** |
---|
| 155 | |
---|
| 156 | // swap row/column i1 with i2 in the n*n matrix A. the leading dimension of |
---|
| 157 | // A is nskip. this only references and swaps the lower triangle. |
---|
| 158 | // if `do_fast_row_swaps' is nonzero and row pointers are being used, then |
---|
| 159 | // rows will be swapped by exchanging row pointers. otherwise the data will |
---|
| 160 | // be copied. |
---|
| 161 | |
---|
| 162 | static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int nskip, |
---|
| 163 | int do_fast_row_swaps) |
---|
| 164 | { |
---|
| 165 | int i; |
---|
| 166 | dAASSERT (A && n > 0 && i1 >= 0 && i2 >= 0 && i1 < n && i2 < n && |
---|
| 167 | nskip >= n && i1 < i2); |
---|
| 168 | |
---|
| 169 | # ifdef ROWPTRS |
---|
| 170 | for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1]; |
---|
| 171 | for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i]; |
---|
| 172 | A[i1][i2] = A[i1][i1]; |
---|
| 173 | A[i1][i1] = A[i2][i1]; |
---|
| 174 | A[i2][i1] = A[i2][i2]; |
---|
| 175 | // swap rows, by swapping row pointers |
---|
| 176 | if (do_fast_row_swaps) { |
---|
| 177 | dReal *tmpp; |
---|
| 178 | tmpp = A[i1]; |
---|
| 179 | A[i1] = A[i2]; |
---|
| 180 | A[i2] = tmpp; |
---|
| 181 | } |
---|
| 182 | else { |
---|
| 183 | ALLOCA (dReal,tmprow,n * sizeof(dReal)); |
---|
| 184 | |
---|
| 185 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 186 | if (tmprow == NULL) { |
---|
| 187 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 188 | return; |
---|
| 189 | } |
---|
| 190 | #endif |
---|
| 191 | |
---|
| 192 | memcpy (tmprow,A[i1],n * sizeof(dReal)); |
---|
| 193 | memcpy (A[i1],A[i2],n * sizeof(dReal)); |
---|
| 194 | memcpy (A[i2],tmprow,n * sizeof(dReal)); |
---|
| 195 | UNALLOCA(tmprow); |
---|
| 196 | } |
---|
| 197 | // swap columns the hard way |
---|
| 198 | for (i=i2+1; i<n; i++) { |
---|
| 199 | dReal tmp = A[i][i1]; |
---|
| 200 | A[i][i1] = A[i][i2]; |
---|
| 201 | A[i][i2] = tmp; |
---|
| 202 | } |
---|
| 203 | # else |
---|
| 204 | dReal tmp; |
---|
| 205 | ALLOCA (dReal,tmprow,n * sizeof(dReal)); |
---|
| 206 | |
---|
| 207 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 208 | if (tmprow == NULL) { |
---|
| 209 | return; |
---|
| 210 | } |
---|
| 211 | #endif |
---|
| 212 | |
---|
| 213 | if (i1 > 0) { |
---|
| 214 | memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal)); |
---|
| 215 | memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal)); |
---|
| 216 | memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal)); |
---|
| 217 | } |
---|
| 218 | for (i=i1+1; i<i2; i++) { |
---|
| 219 | tmp = A[i2*nskip+i]; |
---|
| 220 | A[i2*nskip+i] = A[i*nskip+i1]; |
---|
| 221 | A[i*nskip+i1] = tmp; |
---|
| 222 | } |
---|
| 223 | tmp = A[i1*nskip+i1]; |
---|
| 224 | A[i1*nskip+i1] = A[i2*nskip+i2]; |
---|
| 225 | A[i2*nskip+i2] = tmp; |
---|
| 226 | for (i=i2+1; i<n; i++) { |
---|
| 227 | tmp = A[i*nskip+i1]; |
---|
| 228 | A[i*nskip+i1] = A[i*nskip+i2]; |
---|
| 229 | A[i*nskip+i2] = tmp; |
---|
| 230 | } |
---|
| 231 | UNALLOCA(tmprow); |
---|
| 232 | # endif |
---|
| 233 | |
---|
| 234 | } |
---|
| 235 | |
---|
| 236 | |
---|
| 237 | // swap two indexes in the n*n LCP problem. i1 must be <= i2. |
---|
| 238 | |
---|
| 239 | static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo, |
---|
| 240 | dReal *hi, int *p, int *state, int *findex, |
---|
| 241 | int n, int i1, int i2, int nskip, |
---|
| 242 | int do_fast_row_swaps) |
---|
| 243 | { |
---|
| 244 | dReal tmp; |
---|
| 245 | int tmpi; |
---|
| 246 | dIASSERT (n>0 && i1 >=0 && i2 >= 0 && i1 < n && i2 < n && nskip >= n && |
---|
| 247 | i1 <= i2); |
---|
| 248 | if (i1==i2) return; |
---|
| 249 | swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps); |
---|
| 250 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 251 | if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) |
---|
| 252 | return; |
---|
| 253 | #endif |
---|
| 254 | tmp = x[i1]; |
---|
| 255 | x[i1] = x[i2]; |
---|
| 256 | x[i2] = tmp; |
---|
| 257 | tmp = b[i1]; |
---|
| 258 | b[i1] = b[i2]; |
---|
| 259 | b[i2] = tmp; |
---|
| 260 | tmp = w[i1]; |
---|
| 261 | w[i1] = w[i2]; |
---|
| 262 | w[i2] = tmp; |
---|
| 263 | tmp = lo[i1]; |
---|
| 264 | lo[i1] = lo[i2]; |
---|
| 265 | lo[i2] = tmp; |
---|
| 266 | tmp = hi[i1]; |
---|
| 267 | hi[i1] = hi[i2]; |
---|
| 268 | hi[i2] = tmp; |
---|
| 269 | tmpi = p[i1]; |
---|
| 270 | p[i1] = p[i2]; |
---|
| 271 | p[i2] = tmpi; |
---|
| 272 | tmpi = state[i1]; |
---|
| 273 | state[i1] = state[i2]; |
---|
| 274 | state[i2] = tmpi; |
---|
| 275 | if (findex) { |
---|
| 276 | tmpi = findex[i1]; |
---|
| 277 | findex[i1] = findex[i2]; |
---|
| 278 | findex[i2] = tmpi; |
---|
| 279 | } |
---|
| 280 | } |
---|
| 281 | |
---|
| 282 | |
---|
| 283 | // for debugging - check that L,d is the factorization of A[C,C]. |
---|
| 284 | // A[C,C] has size nC*nC and leading dimension nskip. |
---|
| 285 | // L has size nC*nC and leading dimension nskip. |
---|
| 286 | // d has size nC. |
---|
| 287 | |
---|
| 288 | #ifdef DEBUG_LCP |
---|
| 289 | |
---|
| 290 | static void checkFactorization (ATYPE A, dReal *_L, dReal *_d, |
---|
| 291 | int nC, int *C, int nskip) |
---|
| 292 | { |
---|
| 293 | int i,j; |
---|
| 294 | if (nC==0) return; |
---|
| 295 | |
---|
| 296 | // get A1=A, copy the lower triangle to the upper triangle, get A2=A[C,C] |
---|
| 297 | dMatrix A1 (nC,nC); |
---|
| 298 | for (i=0; i<nC; i++) { |
---|
| 299 | for (j=0; j<=i; j++) A1(i,j) = A1(j,i) = AROW(i)[j]; |
---|
| 300 | } |
---|
| 301 | dMatrix A2 = A1.select (nC,C,nC,C); |
---|
| 302 | |
---|
| 303 | // printf ("A1=\n"); A1.print(); printf ("\n"); |
---|
| 304 | // printf ("A2=\n"); A2.print(); printf ("\n"); |
---|
| 305 | |
---|
| 306 | // compute A3 = L*D*L' |
---|
| 307 | dMatrix L (nC,nC,_L,nskip,1); |
---|
| 308 | dMatrix D (nC,nC); |
---|
| 309 | for (i=0; i<nC; i++) D(i,i) = 1/_d[i]; |
---|
| 310 | L.clearUpperTriangle(); |
---|
| 311 | for (i=0; i<nC; i++) L(i,i) = 1; |
---|
| 312 | dMatrix A3 = L * D * L.transpose(); |
---|
| 313 | |
---|
| 314 | // printf ("L=\n"); L.print(); printf ("\n"); |
---|
| 315 | // printf ("D=\n"); D.print(); printf ("\n"); |
---|
| 316 | // printf ("A3=\n"); A2.print(); printf ("\n"); |
---|
| 317 | |
---|
| 318 | // compare A2 and A3 |
---|
| 319 | dReal diff = A2.maxDifference (A3); |
---|
| 320 | if (diff > 1e-8) |
---|
| 321 | dDebug (0,"L*D*L' check, maximum difference = %.6e\n",diff); |
---|
| 322 | } |
---|
| 323 | |
---|
| 324 | #endif |
---|
| 325 | |
---|
| 326 | |
---|
| 327 | // for debugging |
---|
| 328 | |
---|
| 329 | #ifdef DEBUG_LCP |
---|
| 330 | |
---|
| 331 | static void checkPermutations (int i, int n, int nC, int nN, int *p, int *C) |
---|
| 332 | { |
---|
| 333 | int j,k; |
---|
| 334 | dIASSERT (nC>=0 && nN>=0 && (nC+nN)==i && i < n); |
---|
| 335 | for (k=0; k<i; k++) dIASSERT (p[k] >= 0 && p[k] < i); |
---|
| 336 | for (k=i; k<n; k++) dIASSERT (p[k] == k); |
---|
| 337 | for (j=0; j<nC; j++) { |
---|
| 338 | int C_is_bad = 1; |
---|
| 339 | for (k=0; k<nC; k++) if (C[k]==j) C_is_bad = 0; |
---|
| 340 | dIASSERT (C_is_bad==0); |
---|
| 341 | } |
---|
| 342 | } |
---|
| 343 | |
---|
| 344 | #endif |
---|
| 345 | |
---|
| 346 | //*************************************************************************** |
---|
| 347 | // dLCP manipulator object. this represents an n*n LCP problem. |
---|
| 348 | // |
---|
| 349 | // two index sets C and N are kept. each set holds a subset of |
---|
| 350 | // the variable indexes 0..n-1. an index can only be in one set. |
---|
| 351 | // initially both sets are empty. |
---|
| 352 | // |
---|
| 353 | // the index set C is special: solutions to A(C,C)\A(C,i) can be generated. |
---|
| 354 | |
---|
| 355 | #ifdef dLCP_SLOW |
---|
| 356 | |
---|
| 357 | // simple but slow implementation of dLCP, for testing the LCP drivers. |
---|
| 358 | |
---|
| 359 | #include "array.h" |
---|
| 360 | |
---|
| 361 | struct dLCP { |
---|
| 362 | int n,nub,nskip; |
---|
| 363 | dReal *Adata,*x,*b,*w,*lo,*hi; // LCP problem data |
---|
| 364 | ATYPE A; // A rows |
---|
| 365 | dArray<int> C,N; // index sets |
---|
| 366 | int last_i_for_solve1; // last i value given to solve1 |
---|
| 367 | |
---|
| 368 | dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, |
---|
| 369 | dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, |
---|
| 370 | dReal *_Dell, dReal *_ell, dReal *_tmp, |
---|
| 371 | int *_state, int *_findex, int *_p, int *_C, dReal **Arows); |
---|
| 372 | // the constructor is given an initial problem description (A,x,b,w) and |
---|
| 373 | // space for other working data (which the caller may allocate on the stack). |
---|
| 374 | // some of this data is specific to the fast dLCP implementation. |
---|
| 375 | // the matrices A and L have size n*n, vectors have size n*1. |
---|
| 376 | // A represents a symmetric matrix but only the lower triangle is valid. |
---|
| 377 | // `nub' is the number of unbounded indexes at the start. all the indexes |
---|
| 378 | // 0..nub-1 will be put into C. |
---|
| 379 | |
---|
| 380 | ~dLCP(); |
---|
| 381 | |
---|
| 382 | int getNub() { return nub; } |
---|
| 383 | // return the value of `nub'. the constructor may want to change it, |
---|
| 384 | // so the caller should find out its new value. |
---|
| 385 | |
---|
| 386 | // transfer functions: transfer index i to the given set (C or N). indexes |
---|
| 387 | // less than `nub' can never be given. A,x,b,w,etc may be permuted by these |
---|
| 388 | // functions, the caller must be robust to this. |
---|
| 389 | |
---|
| 390 | void transfer_i_to_C (int i); |
---|
| 391 | // this assumes C and N span 1:i-1. this also assumes that solve1() has |
---|
| 392 | // been recently called for the same i without any other transfer |
---|
| 393 | // functions in between (thereby allowing some data reuse for the fast |
---|
| 394 | // implementation). |
---|
| 395 | void transfer_i_to_N (int i); |
---|
| 396 | // this assumes C and N span 1:i-1. |
---|
| 397 | void transfer_i_from_N_to_C (int i); |
---|
| 398 | void transfer_i_from_C_to_N (int i); |
---|
| 399 | |
---|
| 400 | int numC(); |
---|
| 401 | int numN(); |
---|
| 402 | // return the number of indexes in set C/N |
---|
| 403 | |
---|
| 404 | int indexC (int i); |
---|
| 405 | int indexN (int i); |
---|
| 406 | // return index i in set C/N. |
---|
| 407 | |
---|
| 408 | // accessor and arithmetic functions. Aij translates as A(i,j), etc. |
---|
| 409 | // make sure that only the lower triangle of A is ever referenced. |
---|
| 410 | |
---|
| 411 | dReal Aii (int i); |
---|
| 412 | dReal AiC_times_qC (int i, dReal *q); |
---|
| 413 | dReal AiN_times_qN (int i, dReal *q); // for all Nj |
---|
| 414 | void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj |
---|
| 415 | void pN_plusequals_ANi (dReal *p, int i, int sign=1); |
---|
| 416 | // for all Nj. sign = +1,-1. assumes i > maximum index in N. |
---|
| 417 | void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q); |
---|
| 418 | void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj |
---|
| 419 | void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); |
---|
| 420 | // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1. |
---|
| 421 | // the fast version of this function computes some data that is needed by |
---|
| 422 | // transfer_i_to_C(). if only_transfer is nonzero then this function |
---|
| 423 | // *only* computes that data, it does not set a(C). |
---|
| 424 | |
---|
| 425 | void unpermute(); |
---|
| 426 | // call this at the end of the LCP function. if the x/w values have been |
---|
| 427 | // permuted then this will unscramble them. |
---|
| 428 | }; |
---|
| 429 | |
---|
| 430 | |
---|
| 431 | dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, |
---|
| 432 | dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, |
---|
| 433 | dReal *_Dell, dReal *_ell, dReal *_tmp, |
---|
| 434 | int *_state, int *_findex, int *_p, int *_C, dReal **Arows) |
---|
| 435 | { |
---|
| 436 | dUASSERT (_findex==0,"slow dLCP object does not support findex array"); |
---|
| 437 | |
---|
| 438 | n = _n; |
---|
| 439 | nub = _nub; |
---|
| 440 | Adata = _Adata; |
---|
| 441 | A = 0; |
---|
| 442 | x = _x; |
---|
| 443 | b = _b; |
---|
| 444 | w = _w; |
---|
| 445 | lo = _lo; |
---|
| 446 | hi = _hi; |
---|
| 447 | nskip = dPAD(n); |
---|
| 448 | dSetZero (x,n); |
---|
| 449 | last_i_for_solve1 = -1; |
---|
| 450 | |
---|
| 451 | int i,j; |
---|
| 452 | C.setSize (n); |
---|
| 453 | N.setSize (n); |
---|
| 454 | for (i=0; i<n; i++) { |
---|
| 455 | C[i] = 0; |
---|
| 456 | N[i] = 0; |
---|
| 457 | } |
---|
| 458 | |
---|
| 459 | # ifdef ROWPTRS |
---|
| 460 | // make matrix row pointers |
---|
| 461 | A = Arows; |
---|
| 462 | for (i=0; i<n; i++) A[i] = Adata + i*nskip; |
---|
| 463 | # else |
---|
| 464 | A = Adata; |
---|
| 465 | # endif |
---|
| 466 | |
---|
| 467 | // lets make A symmetric |
---|
| 468 | for (i=0; i<n; i++) { |
---|
| 469 | for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i]; |
---|
| 470 | } |
---|
| 471 | |
---|
| 472 | // if nub>0, put all indexes 0..nub-1 into C and solve for x |
---|
| 473 | if (nub > 0) { |
---|
| 474 | for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal)); |
---|
| 475 | dFactorLDLT (_L,_d,nub,nskip); |
---|
| 476 | memcpy (x,b,nub*sizeof(dReal)); |
---|
| 477 | dSolveLDLT (_L,_d,x,nub,nskip); |
---|
| 478 | dSetZero (_w,nub); |
---|
| 479 | for (i=0; i<nub; i++) C[i] = 1; |
---|
| 480 | } |
---|
| 481 | } |
---|
| 482 | |
---|
| 483 | |
---|
| 484 | dLCP::~dLCP() |
---|
| 485 | { |
---|
| 486 | } |
---|
| 487 | |
---|
| 488 | |
---|
| 489 | void dLCP::transfer_i_to_C (int i) |
---|
| 490 | { |
---|
| 491 | if (i < nub) dDebug (0,"bad i"); |
---|
| 492 | if (C[i]) dDebug (0,"i already in C"); |
---|
| 493 | if (N[i]) dDebug (0,"i already in N"); |
---|
| 494 | for (int k=0; k<i; k++) { |
---|
| 495 | if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated"); |
---|
| 496 | } |
---|
| 497 | for (int k=i; k<n; k++) |
---|
| 498 | if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated"); |
---|
| 499 | if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated"); |
---|
| 500 | last_i_for_solve1 = -1; |
---|
| 501 | C[i] = 1; |
---|
| 502 | } |
---|
| 503 | |
---|
| 504 | |
---|
| 505 | void dLCP::transfer_i_to_N (int i) |
---|
| 506 | { |
---|
| 507 | if (i < nub) dDebug (0,"bad i"); |
---|
| 508 | if (C[i]) dDebug (0,"i already in C"); |
---|
| 509 | if (N[i]) dDebug (0,"i already in N"); |
---|
| 510 | for (int k=0; k<i; k++) |
---|
| 511 | if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated"); |
---|
| 512 | for (int k=i; k<n; k++) |
---|
| 513 | if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated"); |
---|
| 514 | last_i_for_solve1 = -1; |
---|
| 515 | N[i] = 1; |
---|
| 516 | } |
---|
| 517 | |
---|
| 518 | |
---|
| 519 | void dLCP::transfer_i_from_N_to_C (int i) |
---|
| 520 | { |
---|
| 521 | if (i < nub) dDebug (0,"bad i"); |
---|
| 522 | if (C[i]) dDebug (0,"i already in C"); |
---|
| 523 | if (!N[i]) dDebug (0,"i not in N"); |
---|
| 524 | last_i_for_solve1 = -1; |
---|
| 525 | N[i] = 0; |
---|
| 526 | C[i] = 1; |
---|
| 527 | } |
---|
| 528 | |
---|
| 529 | |
---|
| 530 | void dLCP::transfer_i_from_C_to_N (int i) |
---|
| 531 | { |
---|
| 532 | if (i < nub) dDebug (0,"bad i"); |
---|
| 533 | if (N[i]) dDebug (0,"i already in N"); |
---|
| 534 | if (!C[i]) dDebug (0,"i not in C"); |
---|
| 535 | last_i_for_solve1 = -1; |
---|
| 536 | C[i] = 0; |
---|
| 537 | N[i] = 1; |
---|
| 538 | } |
---|
| 539 | |
---|
| 540 | |
---|
| 541 | int dLCP::numC() |
---|
| 542 | { |
---|
| 543 | int i,count=0; |
---|
| 544 | for (i=0; i<n; i++) if (C[i]) count++; |
---|
| 545 | return count; |
---|
| 546 | } |
---|
| 547 | |
---|
| 548 | |
---|
| 549 | int dLCP::numN() |
---|
| 550 | { |
---|
| 551 | int i,count=0; |
---|
| 552 | for (i=0; i<n; i++) if (N[i]) count++; |
---|
| 553 | return count; |
---|
| 554 | } |
---|
| 555 | |
---|
| 556 | |
---|
| 557 | int dLCP::indexC (int i) |
---|
| 558 | { |
---|
| 559 | int k,count=0; |
---|
| 560 | for (k=0; k<n; k++) { |
---|
| 561 | if (C[k]) { |
---|
| 562 | if (count==i) return k; |
---|
| 563 | count++; |
---|
| 564 | } |
---|
| 565 | } |
---|
| 566 | dDebug (0,"bad index C (%d)",i); |
---|
| 567 | return 0; |
---|
| 568 | } |
---|
| 569 | |
---|
| 570 | |
---|
| 571 | int dLCP::indexN (int i) |
---|
| 572 | { |
---|
| 573 | int k,count=0; |
---|
| 574 | for (k=0; k<n; k++) { |
---|
| 575 | if (N[k]) { |
---|
| 576 | if (count==i) return k; |
---|
| 577 | count++; |
---|
| 578 | } |
---|
| 579 | } |
---|
| 580 | dDebug (0,"bad index into N"); |
---|
| 581 | return 0; |
---|
| 582 | } |
---|
| 583 | |
---|
| 584 | |
---|
| 585 | dReal dLCP::Aii (int i) |
---|
| 586 | { |
---|
| 587 | return AROW(i)[i]; |
---|
| 588 | } |
---|
| 589 | |
---|
| 590 | |
---|
| 591 | dReal dLCP::AiC_times_qC (int i, dReal *q) |
---|
| 592 | { |
---|
| 593 | dReal sum = 0; |
---|
| 594 | for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k]; |
---|
| 595 | return sum; |
---|
| 596 | } |
---|
| 597 | |
---|
| 598 | |
---|
| 599 | dReal dLCP::AiN_times_qN (int i, dReal *q) |
---|
| 600 | { |
---|
| 601 | dReal sum = 0; |
---|
| 602 | for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k]; |
---|
| 603 | return sum; |
---|
| 604 | } |
---|
| 605 | |
---|
| 606 | |
---|
| 607 | void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q) |
---|
| 608 | { |
---|
| 609 | dReal sum; |
---|
| 610 | for (int ii=0; ii<n; ii++) if (N[ii]) { |
---|
| 611 | sum = 0; |
---|
| 612 | for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj]; |
---|
| 613 | p[ii] = sum; |
---|
| 614 | } |
---|
| 615 | } |
---|
| 616 | |
---|
| 617 | |
---|
| 618 | void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign) |
---|
| 619 | { |
---|
| 620 | int k; |
---|
| 621 | for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated"); |
---|
| 622 | if (sign > 0) { |
---|
| 623 | for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k]; |
---|
| 624 | } |
---|
| 625 | else { |
---|
| 626 | for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k]; |
---|
| 627 | } |
---|
| 628 | } |
---|
| 629 | |
---|
| 630 | |
---|
| 631 | void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q) |
---|
| 632 | { |
---|
| 633 | for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k]; |
---|
| 634 | } |
---|
| 635 | |
---|
| 636 | |
---|
| 637 | void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q) |
---|
| 638 | { |
---|
| 639 | for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k]; |
---|
| 640 | } |
---|
| 641 | |
---|
| 642 | |
---|
| 643 | void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) |
---|
| 644 | { |
---|
| 645 | |
---|
| 646 | ALLOCA (dReal,AA,n*nskip*sizeof(dReal)); |
---|
| 647 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 648 | if (AA == NULL) { |
---|
| 649 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 650 | return; |
---|
| 651 | } |
---|
| 652 | #endif |
---|
| 653 | ALLOCA (dReal,dd,n*sizeof(dReal)); |
---|
| 654 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 655 | if (dd == NULL) { |
---|
| 656 | UNALLOCA(AA); |
---|
| 657 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 658 | return; |
---|
| 659 | } |
---|
| 660 | #endif |
---|
| 661 | ALLOCA (dReal,bb,n*sizeof(dReal)); |
---|
| 662 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 663 | if (bb == NULL) { |
---|
| 664 | UNALLOCA(AA); |
---|
| 665 | UNALLOCA(dd); |
---|
| 666 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 667 | return; |
---|
| 668 | } |
---|
| 669 | #endif |
---|
| 670 | |
---|
| 671 | int ii,jj,AAi,AAj; |
---|
| 672 | |
---|
| 673 | last_i_for_solve1 = i; |
---|
| 674 | AAi = 0; |
---|
| 675 | for (ii=0; ii<n; ii++) if (C[ii]) { |
---|
| 676 | AAj = 0; |
---|
| 677 | for (jj=0; jj<n; jj++) if (C[jj]) { |
---|
| 678 | AA[AAi*nskip+AAj] = AROW(ii)[jj]; |
---|
| 679 | AAj++; |
---|
| 680 | } |
---|
| 681 | bb[AAi] = AROW(i)[ii]; |
---|
| 682 | AAi++; |
---|
| 683 | } |
---|
| 684 | if (AAi==0) { |
---|
| 685 | UNALLOCA (AA); |
---|
| 686 | UNALLOCA (dd); |
---|
| 687 | UNALLOCA (bb); |
---|
| 688 | return; |
---|
| 689 | } |
---|
| 690 | |
---|
| 691 | dFactorLDLT (AA,dd,AAi,nskip); |
---|
| 692 | dSolveLDLT (AA,dd,bb,AAi,nskip); |
---|
| 693 | |
---|
| 694 | AAi=0; |
---|
| 695 | if (dir > 0) { |
---|
| 696 | for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++]; |
---|
| 697 | } |
---|
| 698 | else { |
---|
| 699 | for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++]; |
---|
| 700 | } |
---|
| 701 | |
---|
| 702 | UNALLOCA (AA); |
---|
| 703 | UNALLOCA (dd); |
---|
| 704 | UNALLOCA (bb); |
---|
| 705 | } |
---|
| 706 | |
---|
| 707 | |
---|
| 708 | void dLCP::unpermute() |
---|
| 709 | { |
---|
| 710 | } |
---|
| 711 | |
---|
| 712 | #endif // dLCP_SLOW |
---|
| 713 | |
---|
| 714 | //*************************************************************************** |
---|
| 715 | // fast implementation of dLCP. see the above definition of dLCP for |
---|
| 716 | // interface comments. |
---|
| 717 | // |
---|
| 718 | // `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is |
---|
| 719 | // permuted as the other vectors/matrices are permuted. |
---|
| 720 | // |
---|
| 721 | // A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have |
---|
| 722 | // contiguous indexes. the don't-care indexes follow N. |
---|
| 723 | // |
---|
| 724 | // an L*D*L' factorization is maintained of A(C,C), and whenever indexes are |
---|
| 725 | // added or removed from the set C the factorization is updated. |
---|
| 726 | // thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A. |
---|
| 727 | // the leading dimension of the matrix L is always `nskip'. |
---|
| 728 | // |
---|
| 729 | // at the start there may be other indexes that are unbounded but are not |
---|
| 730 | // included in `nub'. dLCP will permute the matrix so that absolutely all |
---|
| 731 | // unbounded vectors are at the start. thus there may be some initial |
---|
| 732 | // permutation. |
---|
| 733 | // |
---|
| 734 | // the algorithms here assume certain patterns, particularly with respect to |
---|
| 735 | // index transfer. |
---|
| 736 | |
---|
| 737 | #ifdef dLCP_FAST |
---|
| 738 | |
---|
| 739 | struct dLCP { |
---|
| 740 | int n,nskip,nub; |
---|
| 741 | ATYPE A; // A rows |
---|
| 742 | dReal *Adata,*x,*b,*w,*lo,*hi; // permuted LCP problem data |
---|
| 743 | dReal *L,*d; // L*D*L' factorization of set C |
---|
| 744 | dReal *Dell,*ell,*tmp; |
---|
| 745 | int *state,*findex,*p,*C; |
---|
| 746 | int nC,nN; // size of each index set |
---|
| 747 | |
---|
| 748 | dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, |
---|
| 749 | dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, |
---|
| 750 | dReal *_Dell, dReal *_ell, dReal *_tmp, |
---|
| 751 | int *_state, int *_findex, int *_p, int *_C, dReal **Arows); |
---|
| 752 | int getNub() { return nub; } |
---|
| 753 | void transfer_i_to_C (int i); |
---|
| 754 | void transfer_i_to_N (int i) |
---|
| 755 | { nN++; } // because we can assume C and N span 1:i-1 |
---|
| 756 | void transfer_i_from_N_to_C (int i); |
---|
| 757 | void transfer_i_from_C_to_N (int i); |
---|
| 758 | int numC() { return nC; } |
---|
| 759 | int numN() { return nN; } |
---|
| 760 | int indexC (int i) { return i; } |
---|
| 761 | int indexN (int i) { return i+nC; } |
---|
| 762 | dReal Aii (int i) { return AROW(i)[i]; } |
---|
| 763 | dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); } |
---|
| 764 | dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); } |
---|
| 765 | void pN_equals_ANC_times_qC (dReal *p, dReal *q); |
---|
| 766 | void pN_plusequals_ANi (dReal *p, int i, int sign=1); |
---|
| 767 | void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q) |
---|
| 768 | { for (int i=0; i<nC; i++) p[i] += s*q[i]; } |
---|
| 769 | void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q) |
---|
| 770 | { for (int i=0; i<nN; i++) p[i+nC] += s*q[i+nC]; } |
---|
| 771 | void solve1 (dReal *a, int i, int dir=1, int only_transfer=0); |
---|
| 772 | void unpermute(); |
---|
| 773 | }; |
---|
| 774 | |
---|
| 775 | |
---|
| 776 | dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w, |
---|
| 777 | dReal *_lo, dReal *_hi, dReal *_L, dReal *_d, |
---|
| 778 | dReal *_Dell, dReal *_ell, dReal *_tmp, |
---|
| 779 | int *_state, int *_findex, int *_p, int *_C, dReal **Arows) |
---|
| 780 | { |
---|
| 781 | n = _n; |
---|
| 782 | nub = _nub; |
---|
| 783 | Adata = _Adata; |
---|
| 784 | A = 0; |
---|
| 785 | x = _x; |
---|
| 786 | b = _b; |
---|
| 787 | w = _w; |
---|
| 788 | lo = _lo; |
---|
| 789 | hi = _hi; |
---|
| 790 | L = _L; |
---|
| 791 | d = _d; |
---|
| 792 | Dell = _Dell; |
---|
| 793 | ell = _ell; |
---|
| 794 | tmp = _tmp; |
---|
| 795 | state = _state; |
---|
| 796 | findex = _findex; |
---|
| 797 | p = _p; |
---|
| 798 | C = _C; |
---|
| 799 | nskip = dPAD(n); |
---|
| 800 | dSetZero (x,n); |
---|
| 801 | |
---|
| 802 | int k; |
---|
| 803 | |
---|
| 804 | # ifdef ROWPTRS |
---|
| 805 | // make matrix row pointers |
---|
| 806 | A = Arows; |
---|
| 807 | for (k=0; k<n; k++) A[k] = Adata + k*nskip; |
---|
| 808 | # else |
---|
| 809 | A = Adata; |
---|
| 810 | # endif |
---|
| 811 | |
---|
| 812 | nC = 0; |
---|
| 813 | nN = 0; |
---|
| 814 | for (k=0; k<n; k++) p[k]=k; // initially unpermuted |
---|
| 815 | |
---|
| 816 | /* |
---|
| 817 | // for testing, we can do some random swaps in the area i > nub |
---|
| 818 | if (nub < n) { |
---|
| 819 | for (k=0; k<100; k++) { |
---|
| 820 | int i1,i2; |
---|
| 821 | do { |
---|
| 822 | i1 = dRandInt(n-nub)+nub; |
---|
| 823 | i2 = dRandInt(n-nub)+nub; |
---|
| 824 | } |
---|
| 825 | while (i1 > i2); |
---|
| 826 | //printf ("--> %d %d\n",i1,i2); |
---|
| 827 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0); |
---|
| 828 | } |
---|
| 829 | } |
---|
| 830 | */ |
---|
| 831 | |
---|
| 832 | // permute the problem so that *all* the unbounded variables are at the |
---|
| 833 | // start, i.e. look for unbounded variables not included in `nub'. we can |
---|
| 834 | // potentially push up `nub' this way and get a bigger initial factorization. |
---|
| 835 | // note that when we swap rows/cols here we must not just swap row pointers, |
---|
| 836 | // as the initial factorization relies on the data being all in one chunk. |
---|
| 837 | // variables that have findex >= 0 are *not* considered to be unbounded even |
---|
| 838 | // if lo=-inf and hi=inf - this is because these limits may change during the |
---|
| 839 | // solution process. |
---|
| 840 | |
---|
| 841 | for (k=nub; k<n; k++) { |
---|
| 842 | if (findex && findex[k] >= 0) continue; |
---|
| 843 | if (lo[k]==-dInfinity && hi[k]==dInfinity) { |
---|
| 844 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0); |
---|
| 845 | nub++; |
---|
| 846 | } |
---|
| 847 | } |
---|
| 848 | |
---|
| 849 | // if there are unbounded variables at the start, factorize A up to that |
---|
| 850 | // point and solve for x. this puts all indexes 0..nub-1 into C. |
---|
| 851 | if (nub > 0) { |
---|
| 852 | for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal)); |
---|
| 853 | dFactorLDLT (L,d,nub,nskip); |
---|
| 854 | memcpy (x,b,nub*sizeof(dReal)); |
---|
| 855 | dSolveLDLT (L,d,x,nub,nskip); |
---|
| 856 | dSetZero (w,nub); |
---|
| 857 | for (k=0; k<nub; k++) C[k] = k; |
---|
| 858 | nC = nub; |
---|
| 859 | } |
---|
| 860 | |
---|
| 861 | // permute the indexes > nub such that all findex variables are at the end |
---|
| 862 | if (findex) { |
---|
| 863 | int num_at_end = 0; |
---|
| 864 | for (k=n-1; k >= nub; k--) { |
---|
| 865 | if (findex[k] >= 0) { |
---|
| 866 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1); |
---|
| 867 | num_at_end++; |
---|
| 868 | } |
---|
| 869 | } |
---|
| 870 | } |
---|
| 871 | |
---|
| 872 | // print info about indexes |
---|
| 873 | /* |
---|
| 874 | for (k=0; k<n; k++) { |
---|
| 875 | if (k<nub) printf ("C"); |
---|
| 876 | else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c"); |
---|
| 877 | else printf ("."); |
---|
| 878 | } |
---|
| 879 | printf ("\n"); |
---|
| 880 | */ |
---|
| 881 | } |
---|
| 882 | |
---|
| 883 | |
---|
| 884 | void dLCP::transfer_i_to_C (int i) |
---|
| 885 | { |
---|
| 886 | int j; |
---|
| 887 | if (nC > 0) { |
---|
| 888 | // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C)) |
---|
| 889 | for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j]; |
---|
| 890 | d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC)); |
---|
| 891 | } |
---|
| 892 | else { |
---|
| 893 | d[0] = dRecip (AROW(i)[i]); |
---|
| 894 | } |
---|
| 895 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1); |
---|
| 896 | C[nC] = nC; |
---|
| 897 | nC++; |
---|
| 898 | |
---|
| 899 | # ifdef DEBUG_LCP |
---|
| 900 | checkFactorization (A,L,d,nC,C,nskip); |
---|
| 901 | if (i < (n-1)) checkPermutations (i+1,n,nC,nN,p,C); |
---|
| 902 | # endif |
---|
| 903 | } |
---|
| 904 | |
---|
| 905 | |
---|
| 906 | void dLCP::transfer_i_from_N_to_C (int i) |
---|
| 907 | { |
---|
| 908 | int j; |
---|
| 909 | if (nC > 0) { |
---|
| 910 | dReal *aptr = AROW(i); |
---|
| 911 | # ifdef NUB_OPTIMIZATIONS |
---|
| 912 | // if nub>0, initial part of aptr unpermuted |
---|
| 913 | for (j=0; j<nub; j++) Dell[j] = aptr[j]; |
---|
| 914 | for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]]; |
---|
| 915 | # else |
---|
| 916 | for (j=0; j<nC; j++) Dell[j] = aptr[C[j]]; |
---|
| 917 | # endif |
---|
| 918 | dSolveL1 (L,Dell,nC,nskip); |
---|
| 919 | for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j]; |
---|
| 920 | for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j]; |
---|
| 921 | d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC)); |
---|
| 922 | } |
---|
| 923 | else { |
---|
| 924 | d[0] = dRecip (AROW(i)[i]); |
---|
| 925 | } |
---|
| 926 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1); |
---|
| 927 | C[nC] = nC; |
---|
| 928 | nN--; |
---|
| 929 | nC++; |
---|
| 930 | |
---|
| 931 | // @@@ TO DO LATER |
---|
| 932 | // if we just finish here then we'll go back and re-solve for |
---|
| 933 | // delta_x. but actually we can be more efficient and incrementally |
---|
| 934 | // update delta_x here. but if we do this, we wont have ell and Dell |
---|
| 935 | // to use in updating the factorization later. |
---|
| 936 | |
---|
| 937 | # ifdef DEBUG_LCP |
---|
| 938 | checkFactorization (A,L,d,nC,C,nskip); |
---|
| 939 | # endif |
---|
| 940 | } |
---|
| 941 | |
---|
| 942 | |
---|
| 943 | void dLCP::transfer_i_from_C_to_N (int i) |
---|
| 944 | { |
---|
| 945 | // remove a row/column from the factorization, and adjust the |
---|
| 946 | // indexes (black magic!) |
---|
| 947 | int j,k; |
---|
| 948 | for (j=0; j<nC; j++) if (C[j]==i) { |
---|
| 949 | dLDLTRemove (A,C,L,d,n,nC,j,nskip); |
---|
| 950 | for (k=0; k<nC; k++) if (C[k]==nC-1) { |
---|
| 951 | C[k] = C[j]; |
---|
| 952 | if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int)); |
---|
| 953 | break; |
---|
| 954 | } |
---|
| 955 | dIASSERT (k < nC); |
---|
| 956 | break; |
---|
| 957 | } |
---|
| 958 | dIASSERT (j < nC); |
---|
| 959 | swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1); |
---|
| 960 | nC--; |
---|
| 961 | nN++; |
---|
| 962 | |
---|
| 963 | # ifdef DEBUG_LCP |
---|
| 964 | checkFactorization (A,L,d,nC,C,nskip); |
---|
| 965 | # endif |
---|
| 966 | } |
---|
| 967 | |
---|
| 968 | |
---|
| 969 | void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q) |
---|
| 970 | { |
---|
| 971 | // we could try to make this matrix-vector multiplication faster using |
---|
| 972 | // outer product matrix tricks, e.g. with the dMultidotX() functions. |
---|
| 973 | // but i tried it and it actually made things slower on random 100x100 |
---|
| 974 | // problems because of the overhead involved. so we'll stick with the |
---|
| 975 | // simple method for now. |
---|
| 976 | for (int i=0; i<nN; i++) p[i+nC] = dDot (AROW(i+nC),q,nC); |
---|
| 977 | } |
---|
| 978 | |
---|
| 979 | |
---|
| 980 | void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign) |
---|
| 981 | { |
---|
| 982 | dReal *aptr = AROW(i)+nC; |
---|
| 983 | if (sign > 0) { |
---|
| 984 | for (int i=0; i<nN; i++) p[i+nC] += aptr[i]; |
---|
| 985 | } |
---|
| 986 | else { |
---|
| 987 | for (int i=0; i<nN; i++) p[i+nC] -= aptr[i]; |
---|
| 988 | } |
---|
| 989 | } |
---|
| 990 | |
---|
| 991 | |
---|
| 992 | void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer) |
---|
| 993 | { |
---|
| 994 | // the `Dell' and `ell' that are computed here are saved. if index i is |
---|
| 995 | // later added to the factorization then they can be reused. |
---|
| 996 | // |
---|
| 997 | // @@@ question: do we need to solve for entire delta_x??? yes, but |
---|
| 998 | // only if an x goes below 0 during the step. |
---|
| 999 | |
---|
| 1000 | int j; |
---|
| 1001 | if (nC > 0) { |
---|
| 1002 | dReal *aptr = AROW(i); |
---|
| 1003 | # ifdef NUB_OPTIMIZATIONS |
---|
| 1004 | // if nub>0, initial part of aptr[] is guaranteed unpermuted |
---|
| 1005 | for (j=0; j<nub; j++) Dell[j] = aptr[j]; |
---|
| 1006 | for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]]; |
---|
| 1007 | # else |
---|
| 1008 | for (j=0; j<nC; j++) Dell[j] = aptr[C[j]]; |
---|
| 1009 | # endif |
---|
| 1010 | dSolveL1 (L,Dell,nC,nskip); |
---|
| 1011 | for (j=0; j<nC; j++) ell[j] = Dell[j] * d[j]; |
---|
| 1012 | |
---|
| 1013 | if (!only_transfer) { |
---|
| 1014 | for (j=0; j<nC; j++) tmp[j] = ell[j]; |
---|
| 1015 | dSolveL1T (L,tmp,nC,nskip); |
---|
| 1016 | if (dir > 0) { |
---|
| 1017 | for (j=0; j<nC; j++) a[C[j]] = -tmp[j]; |
---|
| 1018 | } |
---|
| 1019 | else { |
---|
| 1020 | for (j=0; j<nC; j++) a[C[j]] = tmp[j]; |
---|
| 1021 | } |
---|
| 1022 | } |
---|
| 1023 | } |
---|
| 1024 | } |
---|
| 1025 | |
---|
| 1026 | |
---|
| 1027 | void dLCP::unpermute() |
---|
| 1028 | { |
---|
| 1029 | // now we have to un-permute x and w |
---|
| 1030 | int j; |
---|
| 1031 | ALLOCA (dReal,tmp,n*sizeof(dReal)); |
---|
| 1032 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1033 | if (tmp == NULL) { |
---|
| 1034 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1035 | return; |
---|
| 1036 | } |
---|
| 1037 | #endif |
---|
| 1038 | memcpy (tmp,x,n*sizeof(dReal)); |
---|
| 1039 | for (j=0; j<n; j++) x[p[j]] = tmp[j]; |
---|
| 1040 | memcpy (tmp,w,n*sizeof(dReal)); |
---|
| 1041 | for (j=0; j<n; j++) w[p[j]] = tmp[j]; |
---|
| 1042 | |
---|
| 1043 | UNALLOCA (tmp); |
---|
| 1044 | } |
---|
| 1045 | |
---|
| 1046 | #endif // dLCP_FAST |
---|
| 1047 | |
---|
| 1048 | //*************************************************************************** |
---|
| 1049 | // an unoptimized Dantzig LCP driver routine for the basic LCP problem. |
---|
| 1050 | // must have lo=0, hi=dInfinity, and nub=0. |
---|
| 1051 | |
---|
| 1052 | void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b, |
---|
| 1053 | dReal *w, int nub, dReal *lo, dReal *hi) |
---|
| 1054 | { |
---|
| 1055 | dAASSERT (n>0 && A && x && b && w && nub == 0); |
---|
| 1056 | |
---|
| 1057 | int i,k; |
---|
| 1058 | int nskip = dPAD(n); |
---|
| 1059 | ALLOCA (dReal,L,n*nskip*sizeof(dReal)); |
---|
| 1060 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1061 | if (L == NULL) { |
---|
| 1062 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1063 | return; |
---|
| 1064 | } |
---|
| 1065 | #endif |
---|
| 1066 | ALLOCA (dReal,d,n*sizeof(dReal)); |
---|
| 1067 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1068 | if (d == NULL) { |
---|
| 1069 | UNALLOCA(L); |
---|
| 1070 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1071 | return; |
---|
| 1072 | } |
---|
| 1073 | #endif |
---|
| 1074 | ALLOCA (dReal,delta_x,n*sizeof(dReal)); |
---|
| 1075 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1076 | if (delta_x == NULL) { |
---|
| 1077 | UNALLOCA(d); |
---|
| 1078 | UNALLOCA(L); |
---|
| 1079 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1080 | return; |
---|
| 1081 | } |
---|
| 1082 | #endif |
---|
| 1083 | ALLOCA (dReal,delta_w,n*sizeof(dReal)); |
---|
| 1084 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1085 | if (delta_w == NULL) { |
---|
| 1086 | UNALLOCA(delta_x); |
---|
| 1087 | UNALLOCA(d); |
---|
| 1088 | UNALLOCA(L); |
---|
| 1089 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1090 | return; |
---|
| 1091 | } |
---|
| 1092 | #endif |
---|
| 1093 | ALLOCA (dReal,Dell,n*sizeof(dReal)); |
---|
| 1094 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1095 | if (Dell == NULL) { |
---|
| 1096 | UNALLOCA(delta_w); |
---|
| 1097 | UNALLOCA(delta_x); |
---|
| 1098 | UNALLOCA(d); |
---|
| 1099 | UNALLOCA(L); |
---|
| 1100 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1101 | return; |
---|
| 1102 | } |
---|
| 1103 | #endif |
---|
| 1104 | ALLOCA (dReal,ell,n*sizeof(dReal)); |
---|
| 1105 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1106 | if (ell == NULL) { |
---|
| 1107 | UNALLOCA(Dell); |
---|
| 1108 | UNALLOCA(delta_w); |
---|
| 1109 | UNALLOCA(delta_x); |
---|
| 1110 | UNALLOCA(d); |
---|
| 1111 | UNALLOCA(L); |
---|
| 1112 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1113 | return; |
---|
| 1114 | } |
---|
| 1115 | #endif |
---|
| 1116 | ALLOCA (dReal,tmp,n*sizeof(dReal)); |
---|
| 1117 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1118 | if (tmp == NULL) { |
---|
| 1119 | UNALLOCA(ell); |
---|
| 1120 | UNALLOCA(Dell); |
---|
| 1121 | UNALLOCA(delta_w); |
---|
| 1122 | UNALLOCA(delta_x); |
---|
| 1123 | UNALLOCA(d); |
---|
| 1124 | UNALLOCA(L); |
---|
| 1125 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1126 | return; |
---|
| 1127 | } |
---|
| 1128 | #endif |
---|
| 1129 | ALLOCA (dReal*,Arows,n*sizeof(dReal*)); |
---|
| 1130 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1131 | if (Arows == NULL) { |
---|
| 1132 | UNALLOCA(tmp); |
---|
| 1133 | UNALLOCA(ell); |
---|
| 1134 | UNALLOCA(Dell); |
---|
| 1135 | UNALLOCA(delta_w); |
---|
| 1136 | UNALLOCA(delta_x); |
---|
| 1137 | UNALLOCA(d); |
---|
| 1138 | UNALLOCA(L); |
---|
| 1139 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1140 | return; |
---|
| 1141 | } |
---|
| 1142 | #endif |
---|
| 1143 | ALLOCA (int,p,n*sizeof(int)); |
---|
| 1144 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1145 | if (p == NULL) { |
---|
| 1146 | UNALLOCA(Arows); |
---|
| 1147 | UNALLOCA(tmp); |
---|
| 1148 | UNALLOCA(ell); |
---|
| 1149 | UNALLOCA(Dell); |
---|
| 1150 | UNALLOCA(delta_w); |
---|
| 1151 | UNALLOCA(delta_x); |
---|
| 1152 | UNALLOCA(d); |
---|
| 1153 | UNALLOCA(L); |
---|
| 1154 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1155 | return; |
---|
| 1156 | } |
---|
| 1157 | #endif |
---|
| 1158 | ALLOCA (int,C,n*sizeof(int)); |
---|
| 1159 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1160 | if (C == NULL) { |
---|
| 1161 | UNALLOCA(p); |
---|
| 1162 | UNALLOCA(Arows); |
---|
| 1163 | UNALLOCA(tmp); |
---|
| 1164 | UNALLOCA(ell); |
---|
| 1165 | UNALLOCA(Dell); |
---|
| 1166 | UNALLOCA(delta_w); |
---|
| 1167 | UNALLOCA(delta_x); |
---|
| 1168 | UNALLOCA(d); |
---|
| 1169 | UNALLOCA(L); |
---|
| 1170 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1171 | return; |
---|
| 1172 | } |
---|
| 1173 | #endif |
---|
| 1174 | ALLOCA (int,dummy,n*sizeof(int)); |
---|
| 1175 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1176 | if (dummy == NULL) { |
---|
| 1177 | UNALLOCA(C); |
---|
| 1178 | UNALLOCA(p); |
---|
| 1179 | UNALLOCA(Arows); |
---|
| 1180 | UNALLOCA(tmp); |
---|
| 1181 | UNALLOCA(ell); |
---|
| 1182 | UNALLOCA(Dell); |
---|
| 1183 | UNALLOCA(delta_w); |
---|
| 1184 | UNALLOCA(delta_x); |
---|
| 1185 | UNALLOCA(d); |
---|
| 1186 | UNALLOCA(L); |
---|
| 1187 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1188 | return; |
---|
| 1189 | } |
---|
| 1190 | #endif |
---|
| 1191 | |
---|
| 1192 | |
---|
| 1193 | dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows); |
---|
| 1194 | nub = lcp.getNub(); |
---|
| 1195 | |
---|
| 1196 | for (i=0; i<n; i++) { |
---|
| 1197 | w[i] = lcp.AiC_times_qC (i,x) - b[i]; |
---|
| 1198 | if (w[i] >= 0) { |
---|
| 1199 | lcp.transfer_i_to_N (i); |
---|
| 1200 | } |
---|
| 1201 | else { |
---|
| 1202 | for (;;) { |
---|
| 1203 | // compute: delta_x(C) = -A(C,C)\A(C,i) |
---|
| 1204 | dSetZero (delta_x,n); |
---|
| 1205 | lcp.solve1 (delta_x,i); |
---|
| 1206 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1207 | if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { |
---|
| 1208 | UNALLOCA(dummy); |
---|
| 1209 | UNALLOCA(C); |
---|
| 1210 | UNALLOCA(p); |
---|
| 1211 | UNALLOCA(Arows); |
---|
| 1212 | UNALLOCA(tmp); |
---|
| 1213 | UNALLOCA(ell); |
---|
| 1214 | UNALLOCA(Dell); |
---|
| 1215 | UNALLOCA(delta_w); |
---|
| 1216 | UNALLOCA(delta_x); |
---|
| 1217 | UNALLOCA(d); |
---|
| 1218 | UNALLOCA(L); |
---|
| 1219 | return; |
---|
| 1220 | } |
---|
| 1221 | #endif |
---|
| 1222 | delta_x[i] = 1; |
---|
| 1223 | |
---|
| 1224 | // compute: delta_w = A*delta_x |
---|
| 1225 | dSetZero (delta_w,n); |
---|
| 1226 | lcp.pN_equals_ANC_times_qC (delta_w,delta_x); |
---|
| 1227 | lcp.pN_plusequals_ANi (delta_w,i); |
---|
| 1228 | delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i); |
---|
| 1229 | |
---|
| 1230 | // find index to switch |
---|
| 1231 | int si = i; // si = switch index |
---|
| 1232 | int si_in_N = 0; // set to 1 if si in N |
---|
| 1233 | dReal s = -w[i]/delta_w[i]; |
---|
| 1234 | |
---|
| 1235 | if (s <= 0) { |
---|
| 1236 | dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); |
---|
| 1237 | if (i < (n-1)) { |
---|
| 1238 | dSetZero (x+i,n-i); |
---|
| 1239 | dSetZero (w+i,n-i); |
---|
| 1240 | } |
---|
| 1241 | goto done; |
---|
| 1242 | } |
---|
| 1243 | |
---|
| 1244 | for (k=0; k < lcp.numN(); k++) { |
---|
| 1245 | if (delta_w[lcp.indexN(k)] < 0) { |
---|
| 1246 | dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; |
---|
| 1247 | if (s2 < s) { |
---|
| 1248 | s = s2; |
---|
| 1249 | si = lcp.indexN(k); |
---|
| 1250 | si_in_N = 1; |
---|
| 1251 | } |
---|
| 1252 | } |
---|
| 1253 | } |
---|
| 1254 | for (k=0; k < lcp.numC(); k++) { |
---|
| 1255 | if (delta_x[lcp.indexC(k)] < 0) { |
---|
| 1256 | dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)]; |
---|
| 1257 | if (s2 < s) { |
---|
| 1258 | s = s2; |
---|
| 1259 | si = lcp.indexC(k); |
---|
| 1260 | si_in_N = 0; |
---|
| 1261 | } |
---|
| 1262 | } |
---|
| 1263 | } |
---|
| 1264 | |
---|
| 1265 | // apply x = x + s * delta_x |
---|
| 1266 | lcp.pC_plusequals_s_times_qC (x,s,delta_x); |
---|
| 1267 | x[i] += s; |
---|
| 1268 | lcp.pN_plusequals_s_times_qN (w,s,delta_w); |
---|
| 1269 | w[i] += s * delta_w[i]; |
---|
| 1270 | |
---|
| 1271 | // switch indexes between sets if necessary |
---|
| 1272 | if (si==i) { |
---|
| 1273 | w[i] = 0; |
---|
| 1274 | lcp.transfer_i_to_C (i); |
---|
| 1275 | break; |
---|
| 1276 | } |
---|
| 1277 | if (si_in_N) { |
---|
| 1278 | w[si] = 0; |
---|
| 1279 | lcp.transfer_i_from_N_to_C (si); |
---|
| 1280 | } |
---|
| 1281 | else { |
---|
| 1282 | x[si] = 0; |
---|
| 1283 | lcp.transfer_i_from_C_to_N (si); |
---|
| 1284 | } |
---|
| 1285 | } |
---|
| 1286 | } |
---|
| 1287 | } |
---|
| 1288 | |
---|
| 1289 | done: |
---|
| 1290 | lcp.unpermute(); |
---|
| 1291 | |
---|
| 1292 | UNALLOCA (L); |
---|
| 1293 | UNALLOCA (d); |
---|
| 1294 | UNALLOCA (delta_x); |
---|
| 1295 | UNALLOCA (delta_w); |
---|
| 1296 | UNALLOCA (Dell); |
---|
| 1297 | UNALLOCA (ell); |
---|
| 1298 | UNALLOCA (tmp); |
---|
| 1299 | UNALLOCA (Arows); |
---|
| 1300 | UNALLOCA (p); |
---|
| 1301 | UNALLOCA (C); |
---|
| 1302 | UNALLOCA (dummy); |
---|
| 1303 | } |
---|
| 1304 | |
---|
| 1305 | //*************************************************************************** |
---|
| 1306 | // an optimized Dantzig LCP driver routine for the lo-hi LCP problem. |
---|
| 1307 | |
---|
| 1308 | void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, |
---|
| 1309 | dReal *w, int nub, dReal *lo, dReal *hi, int *findex) |
---|
| 1310 | { |
---|
| 1311 | dAASSERT (n>0 && A && x && b && w && lo && hi && nub >= 0 && nub <= n); |
---|
| 1312 | |
---|
| 1313 | int i,k,hit_first_friction_index = 0; |
---|
| 1314 | int nskip = dPAD(n); |
---|
| 1315 | |
---|
| 1316 | // if all the variables are unbounded then we can just factor, solve, |
---|
| 1317 | // and return |
---|
| 1318 | if (nub >= n) { |
---|
| 1319 | dFactorLDLT (A,w,n,nskip); // use w for d |
---|
| 1320 | dSolveLDLT (A,w,b,n,nskip); |
---|
| 1321 | memcpy (x,b,n*sizeof(dReal)); |
---|
| 1322 | dSetZero (w,n); |
---|
| 1323 | |
---|
| 1324 | return; |
---|
| 1325 | } |
---|
| 1326 | # ifndef dNODEBUG |
---|
| 1327 | // check restrictions on lo and hi |
---|
| 1328 | for (k=0; k<n; k++) dIASSERT (lo[k] <= 0 && hi[k] >= 0); |
---|
| 1329 | # endif |
---|
| 1330 | ALLOCA (dReal,L,n*nskip*sizeof(dReal)); |
---|
| 1331 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1332 | if (L == NULL) { |
---|
| 1333 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1334 | return; |
---|
| 1335 | } |
---|
| 1336 | #endif |
---|
| 1337 | ALLOCA (dReal,d,n*sizeof(dReal)); |
---|
| 1338 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1339 | if (d == NULL) { |
---|
| 1340 | UNALLOCA(L); |
---|
| 1341 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1342 | return; |
---|
| 1343 | } |
---|
| 1344 | #endif |
---|
| 1345 | ALLOCA (dReal,delta_x,n*sizeof(dReal)); |
---|
| 1346 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1347 | if (delta_x == NULL) { |
---|
| 1348 | UNALLOCA(d); |
---|
| 1349 | UNALLOCA(L); |
---|
| 1350 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1351 | return; |
---|
| 1352 | } |
---|
| 1353 | #endif |
---|
| 1354 | ALLOCA (dReal,delta_w,n*sizeof(dReal)); |
---|
| 1355 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1356 | if (delta_w == NULL) { |
---|
| 1357 | UNALLOCA(delta_x); |
---|
| 1358 | UNALLOCA(d); |
---|
| 1359 | UNALLOCA(L); |
---|
| 1360 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1361 | return; |
---|
| 1362 | } |
---|
| 1363 | #endif |
---|
| 1364 | ALLOCA (dReal,Dell,n*sizeof(dReal)); |
---|
| 1365 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1366 | if (Dell == NULL) { |
---|
| 1367 | UNALLOCA(delta_w); |
---|
| 1368 | UNALLOCA(delta_x); |
---|
| 1369 | UNALLOCA(d); |
---|
| 1370 | UNALLOCA(L); |
---|
| 1371 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1372 | return; |
---|
| 1373 | } |
---|
| 1374 | #endif |
---|
| 1375 | ALLOCA (dReal,ell,n*sizeof(dReal)); |
---|
| 1376 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1377 | if (ell == NULL) { |
---|
| 1378 | UNALLOCA(Dell); |
---|
| 1379 | UNALLOCA(delta_w); |
---|
| 1380 | UNALLOCA(delta_x); |
---|
| 1381 | UNALLOCA(d); |
---|
| 1382 | UNALLOCA(L); |
---|
| 1383 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1384 | return; |
---|
| 1385 | } |
---|
| 1386 | #endif |
---|
| 1387 | ALLOCA (dReal*,Arows,n*sizeof(dReal*)); |
---|
| 1388 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1389 | if (Arows == NULL) { |
---|
| 1390 | UNALLOCA(ell); |
---|
| 1391 | UNALLOCA(Dell); |
---|
| 1392 | UNALLOCA(delta_w); |
---|
| 1393 | UNALLOCA(delta_x); |
---|
| 1394 | UNALLOCA(d); |
---|
| 1395 | UNALLOCA(L); |
---|
| 1396 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1397 | return; |
---|
| 1398 | } |
---|
| 1399 | #endif |
---|
| 1400 | ALLOCA (int,p,n*sizeof(int)); |
---|
| 1401 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1402 | if (p == NULL) { |
---|
| 1403 | UNALLOCA(Arows); |
---|
| 1404 | UNALLOCA(ell); |
---|
| 1405 | UNALLOCA(Dell); |
---|
| 1406 | UNALLOCA(delta_w); |
---|
| 1407 | UNALLOCA(delta_x); |
---|
| 1408 | UNALLOCA(d); |
---|
| 1409 | UNALLOCA(L); |
---|
| 1410 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1411 | return; |
---|
| 1412 | } |
---|
| 1413 | #endif |
---|
| 1414 | ALLOCA (int,C,n*sizeof(int)); |
---|
| 1415 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1416 | if (C == NULL) { |
---|
| 1417 | UNALLOCA(p); |
---|
| 1418 | UNALLOCA(Arows); |
---|
| 1419 | UNALLOCA(ell); |
---|
| 1420 | UNALLOCA(Dell); |
---|
| 1421 | UNALLOCA(delta_w); |
---|
| 1422 | UNALLOCA(delta_x); |
---|
| 1423 | UNALLOCA(d); |
---|
| 1424 | UNALLOCA(L); |
---|
| 1425 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1426 | return; |
---|
| 1427 | } |
---|
| 1428 | #endif |
---|
| 1429 | |
---|
| 1430 | int dir; |
---|
| 1431 | dReal dirf; |
---|
| 1432 | |
---|
| 1433 | // for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i) |
---|
| 1434 | ALLOCA (int,state,n*sizeof(int)); |
---|
| 1435 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1436 | if (state == NULL) { |
---|
| 1437 | UNALLOCA(C); |
---|
| 1438 | UNALLOCA(p); |
---|
| 1439 | UNALLOCA(Arows); |
---|
| 1440 | UNALLOCA(ell); |
---|
| 1441 | UNALLOCA(Dell); |
---|
| 1442 | UNALLOCA(delta_w); |
---|
| 1443 | UNALLOCA(delta_x); |
---|
| 1444 | UNALLOCA(d); |
---|
| 1445 | UNALLOCA(L); |
---|
| 1446 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1447 | return; |
---|
| 1448 | } |
---|
| 1449 | #endif |
---|
| 1450 | |
---|
| 1451 | // create LCP object. note that tmp is set to delta_w to save space, this |
---|
| 1452 | // optimization relies on knowledge of how tmp is used, so be careful! |
---|
| 1453 | dLCP *lcp=new dLCP(n,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows); |
---|
| 1454 | nub = lcp->getNub(); |
---|
| 1455 | |
---|
| 1456 | // loop over all indexes nub..n-1. for index i, if x(i),w(i) satisfy the |
---|
| 1457 | // LCP conditions then i is added to the appropriate index set. otherwise |
---|
| 1458 | // x(i),w(i) is driven either +ve or -ve to force it to the valid region. |
---|
| 1459 | // as we drive x(i), x(C) is also adjusted to keep w(C) at zero. |
---|
| 1460 | // while driving x(i) we maintain the LCP conditions on the other variables |
---|
| 1461 | // 0..i-1. we do this by watching out for other x(i),w(i) values going |
---|
| 1462 | // outside the valid region, and then switching them between index sets |
---|
| 1463 | // when that happens. |
---|
| 1464 | |
---|
| 1465 | for (i=nub; i<n; i++) { |
---|
| 1466 | // the index i is the driving index and indexes i+1..n-1 are "dont care", |
---|
| 1467 | // i.e. when we make changes to the system those x's will be zero and we |
---|
| 1468 | // don't care what happens to those w's. in other words, we only consider |
---|
| 1469 | // an (i+1)*(i+1) sub-problem of A*x=b+w. |
---|
| 1470 | |
---|
| 1471 | // if we've hit the first friction index, we have to compute the lo and |
---|
| 1472 | // hi values based on the values of x already computed. we have been |
---|
| 1473 | // permuting the indexes, so the values stored in the findex vector are |
---|
| 1474 | // no longer valid. thus we have to temporarily unpermute the x vector. |
---|
| 1475 | // for the purposes of this computation, 0*infinity = 0 ... so if the |
---|
| 1476 | // contact constraint's normal force is 0, there should be no tangential |
---|
| 1477 | // force applied. |
---|
| 1478 | |
---|
| 1479 | if (hit_first_friction_index == 0 && findex && findex[i] >= 0) { |
---|
| 1480 | // un-permute x into delta_w, which is not being used at the moment |
---|
| 1481 | for (k=0; k<n; k++) delta_w[p[k]] = x[k]; |
---|
| 1482 | |
---|
| 1483 | // set lo and hi values |
---|
| 1484 | for (k=i; k<n; k++) { |
---|
| 1485 | dReal wfk = delta_w[findex[k]]; |
---|
| 1486 | if (wfk == 0) { |
---|
| 1487 | hi[k] = 0; |
---|
| 1488 | lo[k] = 0; |
---|
| 1489 | } |
---|
| 1490 | else { |
---|
| 1491 | hi[k] = dFabs (hi[k] * wfk); |
---|
| 1492 | lo[k] = -hi[k]; |
---|
| 1493 | } |
---|
| 1494 | } |
---|
| 1495 | hit_first_friction_index = 1; |
---|
| 1496 | } |
---|
| 1497 | |
---|
| 1498 | // thus far we have not even been computing the w values for indexes |
---|
| 1499 | // greater than i, so compute w[i] now. |
---|
| 1500 | w[i] = lcp->AiC_times_qC (i,x) + lcp->AiN_times_qN (i,x) - b[i]; |
---|
| 1501 | |
---|
| 1502 | // if lo=hi=0 (which can happen for tangential friction when normals are |
---|
| 1503 | // 0) then the index will be assigned to set N with some state. however, |
---|
| 1504 | // set C's line has zero size, so the index will always remain in set N. |
---|
| 1505 | // with the "normal" switching logic, if w changed sign then the index |
---|
| 1506 | // would have to switch to set C and then back to set N with an inverted |
---|
| 1507 | // state. this is pointless, and also computationally expensive. to |
---|
| 1508 | // prevent this from happening, we use the rule that indexes with lo=hi=0 |
---|
| 1509 | // will never be checked for set changes. this means that the state for |
---|
| 1510 | // these indexes may be incorrect, but that doesn't matter. |
---|
| 1511 | |
---|
| 1512 | // see if x(i),w(i) is in a valid region |
---|
| 1513 | if (lo[i]==0 && w[i] >= 0) { |
---|
| 1514 | lcp->transfer_i_to_N (i); |
---|
| 1515 | state[i] = 0; |
---|
| 1516 | } |
---|
| 1517 | else if (hi[i]==0 && w[i] <= 0) { |
---|
| 1518 | lcp->transfer_i_to_N (i); |
---|
| 1519 | state[i] = 1; |
---|
| 1520 | } |
---|
| 1521 | else if (w[i]==0) { |
---|
| 1522 | // this is a degenerate case. by the time we get to this test we know |
---|
| 1523 | // that lo != 0, which means that lo < 0 as lo is not allowed to be +ve, |
---|
| 1524 | // and similarly that hi > 0. this means that the line segment |
---|
| 1525 | // corresponding to set C is at least finite in extent, and we are on it. |
---|
| 1526 | // NOTE: we must call lcp->solve1() before lcp->transfer_i_to_C() |
---|
| 1527 | lcp->solve1 (delta_x,i,0,1); |
---|
| 1528 | |
---|
| 1529 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1530 | if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { |
---|
| 1531 | UNALLOCA(state); |
---|
| 1532 | UNALLOCA(C); |
---|
| 1533 | UNALLOCA(p); |
---|
| 1534 | UNALLOCA(Arows); |
---|
| 1535 | UNALLOCA(ell); |
---|
| 1536 | UNALLOCA(Dell); |
---|
| 1537 | UNALLOCA(delta_w); |
---|
| 1538 | UNALLOCA(delta_x); |
---|
| 1539 | UNALLOCA(d); |
---|
| 1540 | UNALLOCA(L); |
---|
| 1541 | return; |
---|
| 1542 | } |
---|
| 1543 | #endif |
---|
| 1544 | |
---|
| 1545 | lcp->transfer_i_to_C (i); |
---|
| 1546 | } |
---|
| 1547 | else { |
---|
| 1548 | // we must push x(i) and w(i) |
---|
| 1549 | for (;;) { |
---|
| 1550 | // find direction to push on x(i) |
---|
| 1551 | if (w[i] <= 0) { |
---|
| 1552 | dir = 1; |
---|
| 1553 | dirf = REAL(1.0); |
---|
| 1554 | } |
---|
| 1555 | else { |
---|
| 1556 | dir = -1; |
---|
| 1557 | dirf = REAL(-1.0); |
---|
| 1558 | } |
---|
| 1559 | |
---|
| 1560 | // compute: delta_x(C) = -dir*A(C,C)\A(C,i) |
---|
| 1561 | lcp->solve1 (delta_x,i,dir); |
---|
| 1562 | |
---|
| 1563 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1564 | if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { |
---|
| 1565 | UNALLOCA(state); |
---|
| 1566 | UNALLOCA(C); |
---|
| 1567 | UNALLOCA(p); |
---|
| 1568 | UNALLOCA(Arows); |
---|
| 1569 | UNALLOCA(ell); |
---|
| 1570 | UNALLOCA(Dell); |
---|
| 1571 | UNALLOCA(delta_w); |
---|
| 1572 | UNALLOCA(delta_x); |
---|
| 1573 | UNALLOCA(d); |
---|
| 1574 | UNALLOCA(L); |
---|
| 1575 | return; |
---|
| 1576 | } |
---|
| 1577 | #endif |
---|
| 1578 | |
---|
| 1579 | // note that delta_x[i] = dirf, but we wont bother to set it |
---|
| 1580 | |
---|
| 1581 | // compute: delta_w = A*delta_x ... note we only care about |
---|
| 1582 | // delta_w(N) and delta_w(i), the rest is ignored |
---|
| 1583 | lcp->pN_equals_ANC_times_qC (delta_w,delta_x); |
---|
| 1584 | lcp->pN_plusequals_ANi (delta_w,i,dir); |
---|
| 1585 | delta_w[i] = lcp->AiC_times_qC (i,delta_x) + lcp->Aii(i)*dirf; |
---|
| 1586 | |
---|
| 1587 | // find largest step we can take (size=s), either to drive x(i),w(i) |
---|
| 1588 | // to the valid LCP region or to drive an already-valid variable |
---|
| 1589 | // outside the valid region. |
---|
| 1590 | |
---|
| 1591 | int cmd = 1; // index switching command |
---|
| 1592 | int si = 0; // si = index to switch if cmd>3 |
---|
| 1593 | dReal s = -w[i]/delta_w[i]; |
---|
| 1594 | if (dir > 0) { |
---|
| 1595 | if (hi[i] < dInfinity) { |
---|
| 1596 | dReal s2 = (hi[i]-x[i])/dirf; // step to x(i)=hi(i) |
---|
| 1597 | if (s2 < s) { |
---|
| 1598 | s = s2; |
---|
| 1599 | cmd = 3; |
---|
| 1600 | } |
---|
| 1601 | } |
---|
| 1602 | } |
---|
| 1603 | else { |
---|
| 1604 | if (lo[i] > -dInfinity) { |
---|
| 1605 | dReal s2 = (lo[i]-x[i])/dirf; // step to x(i)=lo(i) |
---|
| 1606 | if (s2 < s) { |
---|
| 1607 | s = s2; |
---|
| 1608 | cmd = 2; |
---|
| 1609 | } |
---|
| 1610 | } |
---|
| 1611 | } |
---|
| 1612 | |
---|
| 1613 | for (k=0; k < lcp->numN(); k++) { |
---|
| 1614 | if ((state[lcp->indexN(k)]==0 && delta_w[lcp->indexN(k)] < 0) || |
---|
| 1615 | (state[lcp->indexN(k)]!=0 && delta_w[lcp->indexN(k)] > 0)) { |
---|
| 1616 | // don't bother checking if lo=hi=0 |
---|
| 1617 | if (lo[lcp->indexN(k)] == 0 && hi[lcp->indexN(k)] == 0) continue; |
---|
| 1618 | dReal s2 = -w[lcp->indexN(k)] / delta_w[lcp->indexN(k)]; |
---|
| 1619 | if (s2 < s) { |
---|
| 1620 | s = s2; |
---|
| 1621 | cmd = 4; |
---|
| 1622 | si = lcp->indexN(k); |
---|
| 1623 | } |
---|
| 1624 | } |
---|
| 1625 | } |
---|
| 1626 | |
---|
| 1627 | for (k=nub; k < lcp->numC(); k++) { |
---|
| 1628 | if (delta_x[lcp->indexC(k)] < 0 && lo[lcp->indexC(k)] > -dInfinity) { |
---|
| 1629 | dReal s2 = (lo[lcp->indexC(k)]-x[lcp->indexC(k)]) / |
---|
| 1630 | delta_x[lcp->indexC(k)]; |
---|
| 1631 | if (s2 < s) { |
---|
| 1632 | s = s2; |
---|
| 1633 | cmd = 5; |
---|
| 1634 | si = lcp->indexC(k); |
---|
| 1635 | } |
---|
| 1636 | } |
---|
| 1637 | if (delta_x[lcp->indexC(k)] > 0 && hi[lcp->indexC(k)] < dInfinity) { |
---|
| 1638 | dReal s2 = (hi[lcp->indexC(k)]-x[lcp->indexC(k)]) / |
---|
| 1639 | delta_x[lcp->indexC(k)]; |
---|
| 1640 | if (s2 < s) { |
---|
| 1641 | s = s2; |
---|
| 1642 | cmd = 6; |
---|
| 1643 | si = lcp->indexC(k); |
---|
| 1644 | } |
---|
| 1645 | } |
---|
| 1646 | } |
---|
| 1647 | |
---|
| 1648 | //static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C", |
---|
| 1649 | // "C->NL","C->NH"}; |
---|
| 1650 | //printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i); |
---|
| 1651 | |
---|
| 1652 | // if s <= 0 then we've got a problem. if we just keep going then |
---|
| 1653 | // we're going to get stuck in an infinite loop. instead, just cross |
---|
| 1654 | // our fingers and exit with the current solution. |
---|
| 1655 | if (s <= 0) { |
---|
| 1656 | dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); |
---|
| 1657 | if (i < (n-1)) { |
---|
| 1658 | dSetZero (x+i,n-i); |
---|
| 1659 | dSetZero (w+i,n-i); |
---|
| 1660 | } |
---|
| 1661 | goto done; |
---|
| 1662 | } |
---|
| 1663 | |
---|
| 1664 | // apply x = x + s * delta_x |
---|
| 1665 | lcp->pC_plusequals_s_times_qC (x,s,delta_x); |
---|
| 1666 | x[i] += s * dirf; |
---|
| 1667 | |
---|
| 1668 | // apply w = w + s * delta_w |
---|
| 1669 | lcp->pN_plusequals_s_times_qN (w,s,delta_w); |
---|
| 1670 | w[i] += s * delta_w[i]; |
---|
| 1671 | |
---|
| 1672 | // switch indexes between sets if necessary |
---|
| 1673 | switch (cmd) { |
---|
| 1674 | case 1: // done |
---|
| 1675 | w[i] = 0; |
---|
| 1676 | lcp->transfer_i_to_C (i); |
---|
| 1677 | break; |
---|
| 1678 | case 2: // done |
---|
| 1679 | x[i] = lo[i]; |
---|
| 1680 | state[i] = 0; |
---|
| 1681 | lcp->transfer_i_to_N (i); |
---|
| 1682 | break; |
---|
| 1683 | case 3: // done |
---|
| 1684 | x[i] = hi[i]; |
---|
| 1685 | state[i] = 1; |
---|
| 1686 | lcp->transfer_i_to_N (i); |
---|
| 1687 | break; |
---|
| 1688 | case 4: // keep going |
---|
| 1689 | w[si] = 0; |
---|
| 1690 | lcp->transfer_i_from_N_to_C (si); |
---|
| 1691 | break; |
---|
| 1692 | case 5: // keep going |
---|
| 1693 | x[si] = lo[si]; |
---|
| 1694 | state[si] = 0; |
---|
| 1695 | lcp->transfer_i_from_C_to_N (si); |
---|
| 1696 | break; |
---|
| 1697 | case 6: // keep going |
---|
| 1698 | x[si] = hi[si]; |
---|
| 1699 | state[si] = 1; |
---|
| 1700 | lcp->transfer_i_from_C_to_N (si); |
---|
| 1701 | break; |
---|
| 1702 | } |
---|
| 1703 | |
---|
| 1704 | if (cmd <= 3) break; |
---|
| 1705 | } |
---|
| 1706 | } |
---|
| 1707 | } |
---|
| 1708 | |
---|
| 1709 | done: |
---|
| 1710 | lcp->unpermute(); |
---|
| 1711 | delete lcp; |
---|
| 1712 | |
---|
| 1713 | UNALLOCA (L); |
---|
| 1714 | UNALLOCA (d); |
---|
| 1715 | UNALLOCA (delta_x); |
---|
| 1716 | UNALLOCA (delta_w); |
---|
| 1717 | UNALLOCA (Dell); |
---|
| 1718 | UNALLOCA (ell); |
---|
| 1719 | UNALLOCA (Arows); |
---|
| 1720 | UNALLOCA (p); |
---|
| 1721 | UNALLOCA (C); |
---|
| 1722 | UNALLOCA (state); |
---|
| 1723 | } |
---|
| 1724 | |
---|
| 1725 | //*************************************************************************** |
---|
| 1726 | // accuracy and timing test |
---|
| 1727 | |
---|
| 1728 | extern "C" ODE_API void dTestSolveLCP() |
---|
| 1729 | { |
---|
| 1730 | int n = 100; |
---|
| 1731 | int i,nskip = dPAD(n); |
---|
| 1732 | #ifdef dDOUBLE |
---|
| 1733 | const dReal tol = REAL(1e-9); |
---|
| 1734 | #endif |
---|
| 1735 | #ifdef dSINGLE |
---|
| 1736 | const dReal tol = REAL(1e-4); |
---|
| 1737 | #endif |
---|
| 1738 | printf ("dTestSolveLCP()\n"); |
---|
| 1739 | |
---|
| 1740 | ALLOCA (dReal,A,n*nskip*sizeof(dReal)); |
---|
| 1741 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1742 | if (A == NULL) { |
---|
| 1743 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1744 | return; |
---|
| 1745 | } |
---|
| 1746 | #endif |
---|
| 1747 | ALLOCA (dReal,x,n*sizeof(dReal)); |
---|
| 1748 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1749 | if (x == NULL) { |
---|
| 1750 | UNALLOCA (A); |
---|
| 1751 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1752 | return; |
---|
| 1753 | } |
---|
| 1754 | #endif |
---|
| 1755 | ALLOCA (dReal,b,n*sizeof(dReal)); |
---|
| 1756 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1757 | if (b == NULL) { |
---|
| 1758 | UNALLOCA (x); |
---|
| 1759 | UNALLOCA (A); |
---|
| 1760 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1761 | return; |
---|
| 1762 | } |
---|
| 1763 | #endif |
---|
| 1764 | ALLOCA (dReal,w,n*sizeof(dReal)); |
---|
| 1765 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1766 | if (w == NULL) { |
---|
| 1767 | UNALLOCA (b); |
---|
| 1768 | UNALLOCA (x); |
---|
| 1769 | UNALLOCA (A); |
---|
| 1770 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1771 | return; |
---|
| 1772 | } |
---|
| 1773 | #endif |
---|
| 1774 | ALLOCA (dReal,lo,n*sizeof(dReal)); |
---|
| 1775 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1776 | if (lo == NULL) { |
---|
| 1777 | UNALLOCA (w); |
---|
| 1778 | UNALLOCA (b); |
---|
| 1779 | UNALLOCA (x); |
---|
| 1780 | UNALLOCA (A); |
---|
| 1781 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1782 | return; |
---|
| 1783 | } |
---|
| 1784 | #endif |
---|
| 1785 | ALLOCA (dReal,hi,n*sizeof(dReal)); |
---|
| 1786 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1787 | if (hi == NULL) { |
---|
| 1788 | UNALLOCA (lo); |
---|
| 1789 | UNALLOCA (w); |
---|
| 1790 | UNALLOCA (b); |
---|
| 1791 | UNALLOCA (x); |
---|
| 1792 | UNALLOCA (A); |
---|
| 1793 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1794 | return; |
---|
| 1795 | } |
---|
| 1796 | #endif |
---|
| 1797 | |
---|
| 1798 | ALLOCA (dReal,A2,n*nskip*sizeof(dReal)); |
---|
| 1799 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1800 | if (A2 == NULL) { |
---|
| 1801 | UNALLOCA (hi); |
---|
| 1802 | UNALLOCA (lo); |
---|
| 1803 | UNALLOCA (w); |
---|
| 1804 | UNALLOCA (b); |
---|
| 1805 | UNALLOCA (x); |
---|
| 1806 | UNALLOCA (A); |
---|
| 1807 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1808 | return; |
---|
| 1809 | } |
---|
| 1810 | #endif |
---|
| 1811 | ALLOCA (dReal,b2,n*sizeof(dReal)); |
---|
| 1812 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1813 | if (b2 == NULL) { |
---|
| 1814 | UNALLOCA (A2); |
---|
| 1815 | UNALLOCA (hi); |
---|
| 1816 | UNALLOCA (lo); |
---|
| 1817 | UNALLOCA (w); |
---|
| 1818 | UNALLOCA (b); |
---|
| 1819 | UNALLOCA (x); |
---|
| 1820 | UNALLOCA (A); |
---|
| 1821 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1822 | return; |
---|
| 1823 | } |
---|
| 1824 | #endif |
---|
| 1825 | ALLOCA (dReal,lo2,n*sizeof(dReal)); |
---|
| 1826 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1827 | if (lo2 == NULL) { |
---|
| 1828 | UNALLOCA (b2); |
---|
| 1829 | UNALLOCA (A2); |
---|
| 1830 | UNALLOCA (hi); |
---|
| 1831 | UNALLOCA (lo); |
---|
| 1832 | UNALLOCA (w); |
---|
| 1833 | UNALLOCA (b); |
---|
| 1834 | UNALLOCA (x); |
---|
| 1835 | UNALLOCA (A); |
---|
| 1836 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1837 | return; |
---|
| 1838 | } |
---|
| 1839 | #endif |
---|
| 1840 | ALLOCA (dReal,hi2,n*sizeof(dReal)); |
---|
| 1841 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1842 | if (hi2 == NULL) { |
---|
| 1843 | UNALLOCA (lo2); |
---|
| 1844 | UNALLOCA (b2); |
---|
| 1845 | UNALLOCA (A2); |
---|
| 1846 | UNALLOCA (hi); |
---|
| 1847 | UNALLOCA (lo); |
---|
| 1848 | UNALLOCA (w); |
---|
| 1849 | UNALLOCA (b); |
---|
| 1850 | UNALLOCA (x); |
---|
| 1851 | UNALLOCA (A); |
---|
| 1852 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1853 | return; |
---|
| 1854 | } |
---|
| 1855 | #endif |
---|
| 1856 | ALLOCA (dReal,tmp1,n*sizeof(dReal)); |
---|
| 1857 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1858 | if (tmp1 == NULL) { |
---|
| 1859 | UNALLOCA (hi2); |
---|
| 1860 | UNALLOCA (lo2); |
---|
| 1861 | UNALLOCA (b2); |
---|
| 1862 | UNALLOCA (A2); |
---|
| 1863 | UNALLOCA (hi); |
---|
| 1864 | UNALLOCA (lo); |
---|
| 1865 | UNALLOCA (w); |
---|
| 1866 | UNALLOCA (b); |
---|
| 1867 | UNALLOCA (x); |
---|
| 1868 | UNALLOCA (A); |
---|
| 1869 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1870 | return; |
---|
| 1871 | } |
---|
| 1872 | #endif |
---|
| 1873 | ALLOCA (dReal,tmp2,n*sizeof(dReal)); |
---|
| 1874 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1875 | if (tmp2 == NULL) { |
---|
| 1876 | UNALLOCA (tmp1); |
---|
| 1877 | UNALLOCA (hi2); |
---|
| 1878 | UNALLOCA (lo2); |
---|
| 1879 | UNALLOCA (b2); |
---|
| 1880 | UNALLOCA (A2); |
---|
| 1881 | UNALLOCA (hi); |
---|
| 1882 | UNALLOCA (lo); |
---|
| 1883 | UNALLOCA (w); |
---|
| 1884 | UNALLOCA (b); |
---|
| 1885 | UNALLOCA (x); |
---|
| 1886 | UNALLOCA (A); |
---|
| 1887 | dMemoryFlag = d_MEMORY_OUT_OF_MEMORY; |
---|
| 1888 | return; |
---|
| 1889 | } |
---|
| 1890 | #endif |
---|
| 1891 | |
---|
| 1892 | double total_time = 0; |
---|
| 1893 | for (int count=0; count < 1000; count++) { |
---|
| 1894 | |
---|
| 1895 | // form (A,b) = a random positive definite LCP problem |
---|
| 1896 | dMakeRandomMatrix (A2,n,n,1.0); |
---|
| 1897 | dMultiply2 (A,A2,A2,n,n,n); |
---|
| 1898 | dMakeRandomMatrix (x,n,1,1.0); |
---|
| 1899 | dMultiply0 (b,A,x,n,n,1); |
---|
| 1900 | for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1); |
---|
| 1901 | |
---|
| 1902 | // choose `nub' in the range 0..n-1 |
---|
| 1903 | int nub = 50; //dRandInt (n); |
---|
| 1904 | |
---|
| 1905 | // make limits |
---|
| 1906 | for (i=0; i<nub; i++) lo[i] = -dInfinity; |
---|
| 1907 | for (i=0; i<nub; i++) hi[i] = dInfinity; |
---|
| 1908 | //for (i=nub; i<n; i++) lo[i] = 0; |
---|
| 1909 | //for (i=nub; i<n; i++) hi[i] = dInfinity; |
---|
| 1910 | //for (i=nub; i<n; i++) lo[i] = -dInfinity; |
---|
| 1911 | //for (i=nub; i<n; i++) hi[i] = 0; |
---|
| 1912 | for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01); |
---|
| 1913 | for (i=nub; i<n; i++) hi[i] = (dRandReal()*REAL(1.0))+REAL(0.01); |
---|
| 1914 | |
---|
| 1915 | // set a few limits to lo=hi=0 |
---|
| 1916 | /* |
---|
| 1917 | for (i=0; i<10; i++) { |
---|
| 1918 | int j = dRandInt (n-nub) + nub; |
---|
| 1919 | lo[j] = 0; |
---|
| 1920 | hi[j] = 0; |
---|
| 1921 | } |
---|
| 1922 | */ |
---|
| 1923 | |
---|
| 1924 | // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for |
---|
| 1925 | // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to |
---|
| 1926 | // ensure that it doesn't get referenced (if it does, the answer will be |
---|
| 1927 | // wrong). |
---|
| 1928 | |
---|
| 1929 | memcpy (A2,A,n*nskip*sizeof(dReal)); |
---|
| 1930 | dClearUpperTriangle (A2,n); |
---|
| 1931 | memcpy (b2,b,n*sizeof(dReal)); |
---|
| 1932 | memcpy (lo2,lo,n*sizeof(dReal)); |
---|
| 1933 | memcpy (hi2,hi,n*sizeof(dReal)); |
---|
| 1934 | dSetZero (x,n); |
---|
| 1935 | dSetZero (w,n); |
---|
| 1936 | |
---|
| 1937 | dStopwatch sw; |
---|
| 1938 | dStopwatchReset (&sw); |
---|
| 1939 | dStopwatchStart (&sw); |
---|
| 1940 | |
---|
| 1941 | dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0); |
---|
| 1942 | #ifdef dUSE_MALLOC_FOR_ALLOCA |
---|
| 1943 | if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) { |
---|
| 1944 | UNALLOCA (tmp2); |
---|
| 1945 | UNALLOCA (tmp1); |
---|
| 1946 | UNALLOCA (hi2); |
---|
| 1947 | UNALLOCA (lo2); |
---|
| 1948 | UNALLOCA (b2); |
---|
| 1949 | UNALLOCA (A2); |
---|
| 1950 | UNALLOCA (hi); |
---|
| 1951 | UNALLOCA (lo); |
---|
| 1952 | UNALLOCA (w); |
---|
| 1953 | UNALLOCA (b); |
---|
| 1954 | UNALLOCA (x); |
---|
| 1955 | UNALLOCA (A); |
---|
| 1956 | return; |
---|
| 1957 | } |
---|
| 1958 | #endif |
---|
| 1959 | |
---|
| 1960 | dStopwatchStop (&sw); |
---|
| 1961 | double time = dStopwatchTime(&sw); |
---|
| 1962 | total_time += time; |
---|
| 1963 | double average = total_time / double(count+1) * 1000.0; |
---|
| 1964 | |
---|
| 1965 | // check the solution |
---|
| 1966 | |
---|
| 1967 | dMultiply0 (tmp1,A,x,n,n,1); |
---|
| 1968 | for (i=0; i<n; i++) tmp2[i] = b[i] + w[i]; |
---|
| 1969 | dReal diff = dMaxDifference (tmp1,tmp2,n,1); |
---|
| 1970 | // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff, |
---|
| 1971 | // diff > tol ? "FAILED" : "passed"); |
---|
| 1972 | if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff); |
---|
| 1973 | int n1=0,n2=0,n3=0; |
---|
| 1974 | for (i=0; i<n; i++) { |
---|
| 1975 | if (x[i]==lo[i] && w[i] >= 0) { |
---|
| 1976 | n1++; // ok |
---|
| 1977 | } |
---|
| 1978 | else if (x[i]==hi[i] && w[i] <= 0) { |
---|
| 1979 | n2++; // ok |
---|
| 1980 | } |
---|
| 1981 | else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) { |
---|
| 1982 | n3++; // ok |
---|
| 1983 | } |
---|
| 1984 | else { |
---|
| 1985 | dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i, |
---|
| 1986 | x[i],w[i],lo[i],hi[i]); |
---|
| 1987 | } |
---|
| 1988 | } |
---|
| 1989 | |
---|
| 1990 | // pacifier |
---|
| 1991 | printf ("passed: NL=%3d NH=%3d C=%3d ",n1,n2,n3); |
---|
| 1992 | printf ("time=%10.3f ms avg=%10.4f\n",time * 1000.0,average); |
---|
| 1993 | } |
---|
| 1994 | |
---|
| 1995 | UNALLOCA (A); |
---|
| 1996 | UNALLOCA (x); |
---|
| 1997 | UNALLOCA (b); |
---|
| 1998 | UNALLOCA (w); |
---|
| 1999 | UNALLOCA (lo); |
---|
| 2000 | UNALLOCA (hi); |
---|
| 2001 | UNALLOCA (A2); |
---|
| 2002 | UNALLOCA (b2); |
---|
| 2003 | UNALLOCA (lo2); |
---|
| 2004 | UNALLOCA (hi2); |
---|
| 2005 | UNALLOCA (tmp1); |
---|
| 2006 | UNALLOCA (tmp2); |
---|
| 2007 | } |
---|