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 | #include <ode/common.h> |
---|
24 | #include <ode/matrix.h> |
---|
25 | |
---|
26 | // misc defines |
---|
27 | #define ALLOCA dALLOCA16 |
---|
28 | |
---|
29 | |
---|
30 | void dSetZero (dReal *a, int n) |
---|
31 | { |
---|
32 | dAASSERT (a && n >= 0); |
---|
33 | while (n > 0) { |
---|
34 | *(a++) = 0; |
---|
35 | n--; |
---|
36 | } |
---|
37 | } |
---|
38 | |
---|
39 | |
---|
40 | void dSetValue (dReal *a, int n, dReal value) |
---|
41 | { |
---|
42 | dAASSERT (a && n >= 0); |
---|
43 | while (n > 0) { |
---|
44 | *(a++) = value; |
---|
45 | n--; |
---|
46 | } |
---|
47 | } |
---|
48 | |
---|
49 | |
---|
50 | void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) |
---|
51 | { |
---|
52 | int i,j,k,qskip,rskip,rpad; |
---|
53 | dAASSERT (A && B && C && p>0 && q>0 && r>0); |
---|
54 | qskip = dPAD(q); |
---|
55 | rskip = dPAD(r); |
---|
56 | rpad = rskip - r; |
---|
57 | dReal sum; |
---|
58 | const dReal *b,*c,*bb; |
---|
59 | bb = B; |
---|
60 | for (i=p; i; i--) { |
---|
61 | for (j=0 ; j<r; j++) { |
---|
62 | c = C + j; |
---|
63 | b = bb; |
---|
64 | sum = 0; |
---|
65 | for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c); |
---|
66 | *(A++) = sum; |
---|
67 | } |
---|
68 | A += rpad; |
---|
69 | bb += qskip; |
---|
70 | } |
---|
71 | } |
---|
72 | |
---|
73 | |
---|
74 | void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) |
---|
75 | { |
---|
76 | int i,j,k,pskip,rskip; |
---|
77 | dReal sum; |
---|
78 | dAASSERT (A && B && C && p>0 && q>0 && r>0); |
---|
79 | pskip = dPAD(p); |
---|
80 | rskip = dPAD(r); |
---|
81 | for (i=0; i<p; i++) { |
---|
82 | for (j=0; j<r; j++) { |
---|
83 | sum = 0; |
---|
84 | for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip]; |
---|
85 | A[i*rskip+j] = sum; |
---|
86 | } |
---|
87 | } |
---|
88 | } |
---|
89 | |
---|
90 | |
---|
91 | void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r) |
---|
92 | { |
---|
93 | int i,j,k,z,rpad,qskip; |
---|
94 | dReal sum; |
---|
95 | const dReal *bb,*cc; |
---|
96 | dAASSERT (A && B && C && p>0 && q>0 && r>0); |
---|
97 | rpad = dPAD(r) - r; |
---|
98 | qskip = dPAD(q); |
---|
99 | bb = B; |
---|
100 | for (i=p; i; i--) { |
---|
101 | cc = C; |
---|
102 | for (j=r; j; j--) { |
---|
103 | z = 0; |
---|
104 | sum = 0; |
---|
105 | for (k=q; k; k--,z++) sum += bb[z] * cc[z]; |
---|
106 | *(A++) = sum; |
---|
107 | cc += qskip; |
---|
108 | } |
---|
109 | A += rpad; |
---|
110 | bb += qskip; |
---|
111 | } |
---|
112 | } |
---|
113 | |
---|
114 | |
---|
115 | int dFactorCholesky (dReal *A, int n) |
---|
116 | { |
---|
117 | int i,j,k,nskip; |
---|
118 | dReal sum,*a,*b,*aa,*bb,*cc,*recip; |
---|
119 | dAASSERT (n > 0 && A); |
---|
120 | nskip = dPAD (n); |
---|
121 | recip = (dReal*) ALLOCA (n * sizeof(dReal)); |
---|
122 | aa = A; |
---|
123 | for (i=0; i<n; i++) { |
---|
124 | bb = A; |
---|
125 | cc = A + i*nskip; |
---|
126 | for (j=0; j<i; j++) { |
---|
127 | sum = *cc; |
---|
128 | a = aa; |
---|
129 | b = bb; |
---|
130 | for (k=j; k; k--) sum -= (*(a++))*(*(b++)); |
---|
131 | *cc = sum * recip[j]; |
---|
132 | bb += nskip; |
---|
133 | cc++; |
---|
134 | } |
---|
135 | sum = *cc; |
---|
136 | a = aa; |
---|
137 | for (k=i; k; k--, a++) sum -= (*a)*(*a); |
---|
138 | if (sum <= REAL(0.0)) return 0; |
---|
139 | *cc = dSqrt(sum); |
---|
140 | recip[i] = dRecip (*cc); |
---|
141 | aa += nskip; |
---|
142 | } |
---|
143 | return 1; |
---|
144 | } |
---|
145 | |
---|
146 | |
---|
147 | void dSolveCholesky (const dReal *L, dReal *b, int n) |
---|
148 | { |
---|
149 | int i,k,nskip; |
---|
150 | dReal sum,*y; |
---|
151 | dAASSERT (n > 0 && L && b); |
---|
152 | nskip = dPAD (n); |
---|
153 | y = (dReal*) ALLOCA (n*sizeof(dReal)); |
---|
154 | for (i=0; i<n; i++) { |
---|
155 | sum = 0; |
---|
156 | for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k]; |
---|
157 | y[i] = (b[i]-sum)/L[i*nskip+i]; |
---|
158 | } |
---|
159 | for (i=n-1; i >= 0; i--) { |
---|
160 | sum = 0; |
---|
161 | for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k]; |
---|
162 | b[i] = (y[i]-sum)/L[i*nskip+i]; |
---|
163 | } |
---|
164 | } |
---|
165 | |
---|
166 | |
---|
167 | int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n) |
---|
168 | { |
---|
169 | int i,j,nskip; |
---|
170 | dReal *L,*x; |
---|
171 | dAASSERT (n > 0 && A && Ainv); |
---|
172 | nskip = dPAD (n); |
---|
173 | L = (dReal*) ALLOCA (nskip*n*sizeof(dReal)); |
---|
174 | memcpy (L,A,nskip*n*sizeof(dReal)); |
---|
175 | x = (dReal*) ALLOCA (n*sizeof(dReal)); |
---|
176 | if (dFactorCholesky (L,n)==0) return 0; |
---|
177 | dSetZero (Ainv,n*nskip); // make sure all padding elements set to 0 |
---|
178 | for (i=0; i<n; i++) { |
---|
179 | for (j=0; j<n; j++) x[j] = 0; |
---|
180 | x[i] = 1; |
---|
181 | dSolveCholesky (L,x,n); |
---|
182 | for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j]; |
---|
183 | } |
---|
184 | return 1; |
---|
185 | } |
---|
186 | |
---|
187 | |
---|
188 | int dIsPositiveDefinite (const dReal *A, int n) |
---|
189 | { |
---|
190 | dReal *Acopy; |
---|
191 | dAASSERT (n > 0 && A); |
---|
192 | int nskip = dPAD (n); |
---|
193 | Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal)); |
---|
194 | memcpy (Acopy,A,nskip*n * sizeof(dReal)); |
---|
195 | return dFactorCholesky (Acopy,n); |
---|
196 | } |
---|
197 | |
---|
198 | |
---|
199 | /***** this has been replaced by a faster version |
---|
200 | void dSolveL1T (const dReal *L, dReal *b, int n, int nskip) |
---|
201 | { |
---|
202 | int i,j; |
---|
203 | dAASSERT (L && b && n >= 0 && nskip >= n); |
---|
204 | dReal sum; |
---|
205 | for (i=n-2; i>=0; i--) { |
---|
206 | sum = 0; |
---|
207 | for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j]; |
---|
208 | b[i] -= sum; |
---|
209 | } |
---|
210 | } |
---|
211 | */ |
---|
212 | |
---|
213 | |
---|
214 | void dVectorScale (dReal *a, const dReal *d, int n) |
---|
215 | { |
---|
216 | dAASSERT (a && d && n >= 0); |
---|
217 | for (int i=0; i<n; i++) a[i] *= d[i]; |
---|
218 | } |
---|
219 | |
---|
220 | |
---|
221 | void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip) |
---|
222 | { |
---|
223 | dAASSERT (L && d && b && n > 0 && nskip >= n); |
---|
224 | dSolveL1 (L,b,n,nskip); |
---|
225 | dVectorScale (b,d,n); |
---|
226 | dSolveL1T (L,b,n,nskip); |
---|
227 | } |
---|
228 | |
---|
229 | |
---|
230 | void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip) |
---|
231 | { |
---|
232 | int j,p; |
---|
233 | dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee; |
---|
234 | dAASSERT (L && d && a && n > 0 && nskip >= n); |
---|
235 | |
---|
236 | if (n < 2) return; |
---|
237 | W1 = (dReal*) ALLOCA (n*sizeof(dReal)); |
---|
238 | W2 = (dReal*) ALLOCA (n*sizeof(dReal)); |
---|
239 | |
---|
240 | W1[0] = 0; |
---|
241 | W2[0] = 0; |
---|
242 | for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2; |
---|
243 | W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2; |
---|
244 | W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2; |
---|
245 | |
---|
246 | alpha1=1; |
---|
247 | alpha2=1; |
---|
248 | |
---|
249 | dee = d[0]; |
---|
250 | alphanew = alpha1 + (W11*W11)*dee; |
---|
251 | dee /= alphanew; |
---|
252 | gamma1 = W11 * dee; |
---|
253 | dee *= alpha1; |
---|
254 | alpha1 = alphanew; |
---|
255 | alphanew = alpha2 - (W21*W21)*dee; |
---|
256 | dee /= alphanew; |
---|
257 | gamma2 = W21 * dee; |
---|
258 | alpha2 = alphanew; |
---|
259 | k1 = REAL(1.0) - W21*gamma1; |
---|
260 | k2 = W21*gamma1*W11 - W21; |
---|
261 | for (p=1; p<n; p++) { |
---|
262 | Wp = W1[p]; |
---|
263 | ell = L[p*nskip]; |
---|
264 | W1[p] = Wp - W11*ell; |
---|
265 | W2[p] = k1*Wp + k2*ell; |
---|
266 | } |
---|
267 | |
---|
268 | for (j=1; j<n; j++) { |
---|
269 | dee = d[j]; |
---|
270 | alphanew = alpha1 + (W1[j]*W1[j])*dee; |
---|
271 | dee /= alphanew; |
---|
272 | gamma1 = W1[j] * dee; |
---|
273 | dee *= alpha1; |
---|
274 | alpha1 = alphanew; |
---|
275 | alphanew = alpha2 - (W2[j]*W2[j])*dee; |
---|
276 | dee /= alphanew; |
---|
277 | gamma2 = W2[j] * dee; |
---|
278 | dee *= alpha2; |
---|
279 | d[j] = dee; |
---|
280 | alpha2 = alphanew; |
---|
281 | |
---|
282 | k1 = W1[j]; |
---|
283 | k2 = W2[j]; |
---|
284 | for (p=j+1; p<n; p++) { |
---|
285 | ell = L[p*nskip+j]; |
---|
286 | Wp = W1[p] - k1 * ell; |
---|
287 | ell += gamma1 * Wp; |
---|
288 | W1[p] = Wp; |
---|
289 | Wp = W2[p] - k2 * ell; |
---|
290 | ell -= gamma2 * Wp; |
---|
291 | W2[p] = Wp; |
---|
292 | L[p*nskip+j] = ell; |
---|
293 | } |
---|
294 | } |
---|
295 | } |
---|
296 | |
---|
297 | |
---|
298 | // macros for dLDLTRemove() for accessing A - either access the matrix |
---|
299 | // directly or access it via row pointers. we are only supposed to reference |
---|
300 | // the lower triangle of A (it is symmetric), but indexes i and j come from |
---|
301 | // permutation vectors so they are not predictable. so do a test on the |
---|
302 | // indexes - this should not slow things down too much, as we don't do this |
---|
303 | // in an inner loop. |
---|
304 | |
---|
305 | #define _GETA(i,j) (A[i][j]) |
---|
306 | //#define _GETA(i,j) (A[(i)*nskip+(j)]) |
---|
307 | #define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i)) |
---|
308 | |
---|
309 | |
---|
310 | void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d, |
---|
311 | int n1, int n2, int r, int nskip) |
---|
312 | { |
---|
313 | int i; |
---|
314 | dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 && |
---|
315 | n1 >= n2 && nskip >= n1); |
---|
316 | #ifndef dNODEBUG |
---|
317 | for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1); |
---|
318 | #endif |
---|
319 | |
---|
320 | if (r==n2-1) { |
---|
321 | return; // deleting last row/col is easy |
---|
322 | } |
---|
323 | else if (r==0) { |
---|
324 | dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal)); |
---|
325 | for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]); |
---|
326 | a[0] += REAL(1.0); |
---|
327 | dLDLTAddTL (L,d,a,n2,nskip); |
---|
328 | } |
---|
329 | else { |
---|
330 | dReal *t = (dReal*) ALLOCA (r * sizeof(dReal)); |
---|
331 | dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal)); |
---|
332 | for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i]; |
---|
333 | for (i=0; i<(n2-r); i++) |
---|
334 | a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]); |
---|
335 | a[0] += REAL(1.0); |
---|
336 | dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip); |
---|
337 | } |
---|
338 | |
---|
339 | // snip out row/column r from L and d |
---|
340 | dRemoveRowCol (L,n2,nskip,r); |
---|
341 | if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal)); |
---|
342 | } |
---|
343 | |
---|
344 | |
---|
345 | void dRemoveRowCol (dReal *A, int n, int nskip, int r) |
---|
346 | { |
---|
347 | int i; |
---|
348 | dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n); |
---|
349 | if (r >= n-1) return; |
---|
350 | if (r > 0) { |
---|
351 | for (i=0; i<r; i++) |
---|
352 | memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal)); |
---|
353 | for (i=r; i<(n-1); i++) |
---|
354 | memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal)); |
---|
355 | } |
---|
356 | for (i=r; i<(n-1); i++) |
---|
357 | memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal)); |
---|
358 | } |
---|