2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / matrix.cpp
1 /*************************************************************************
2  *                                                                       *
3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
5  *                                                                       *
6  * This library is free software; you can redistribute it and/or         *
7  * modify it under the terms of EITHER:                                  *
8  *   (1) The GNU Lesser General Public License as published by the Free  *
9  *       Software Foundation; either version 2.1 of the License, or (at  *
10  *       your option) any later version. The text of the GNU Lesser      *
11  *       General Public License is included with this library in the     *
12  *       file LICENSE.TXT.                                               *
13  *   (2) The BSD-style license that is included with this library in     *
14  *       the file LICENSE-BSD.TXT.                                       *
15  *                                                                       *
16  * This library is distributed in the hope that it will be useful,       *
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
20  *                                                                       *
21  *************************************************************************/
22
23 #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 }