aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/matrix.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:20:48 +0000
committerdan miller2007-10-19 05:20:48 +0000
commitd48ea5bb797037069d641da41da0f195f0124491 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/ode/src/matrix.cpp
parentdont ask (diff)
downloadopensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.zip
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.gz
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.bz2
opensim-SC_OLD-d48ea5bb797037069d641da41da0f195f0124491.tar.xz
one more for the gipper
Diffstat (limited to 'libraries/ode-0.9/ode/src/matrix.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/matrix.cpp358
1 files changed, 358 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/matrix.cpp b/libraries/ode-0.9/ode/src/matrix.cpp
new file mode 100644
index 0000000..16afe91
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/matrix.cpp
@@ -0,0 +1,358 @@
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
30void dSetZero (dReal *a, int n)
31{
32 dAASSERT (a && n >= 0);
33 while (n > 0) {
34 *(a++) = 0;
35 n--;
36 }
37}
38
39
40void 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
50void 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
74void 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
91void 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
115int 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
147void 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
167int 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
188int 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
200void 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
214void 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
221void 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
230void 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
310void 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
345void 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}