2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / fastltsolve.c
1 /* generated code, do not edit. */
2
3 #include "ode/matrix.h"
4
5 /* solve L^T * x=b, with b containing 1 right hand side.
6  * L is an n*n lower triangular matrix with ones on the diagonal.
7  * L is stored by rows and its leading dimension is lskip.
8  * b is an n*1 matrix that contains the right hand side.
9  * b is overwritten with x.
10  * this processes blocks of 4.
11  */
12
13 void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
14 {  
15   /* declare variables - Z matrix, p and q vectors, etc */
16   dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
17   const dReal *ell;
18   int lskip2,lskip3,i,j;
19   /* special handling for L and B because we're solving L1 *transpose* */
20   L = L + (n-1)*(lskip1+1);
21   B = B + n-1;
22   lskip1 = -lskip1;
23   /* compute lskip values */
24   lskip2 = 2*lskip1;
25   lskip3 = 3*lskip1;
26   /* compute all 4 x 1 blocks of X */
27   for (i=0; i <= n-4; i+=4) {
28     /* compute all 4 x 1 block of X, from rows i..i+4-1 */
29     /* set the Z matrix to 0 */
30     Z11=0;
31     Z21=0;
32     Z31=0;
33     Z41=0;
34     ell = L - i;
35     ex = B;
36     /* the inner loop that computes outer products and adds them to Z */
37     for (j=i-4; j >= 0; j -= 4) {
38       /* load p and q values */
39       p1=ell[0];
40       q1=ex[0];
41       p2=ell[-1];
42       p3=ell[-2];
43       p4=ell[-3];
44       /* compute outer product and add it to the Z matrix */
45       m11 = p1 * q1;
46       m21 = p2 * q1;
47       m31 = p3 * q1;
48       m41 = p4 * q1;
49       ell += lskip1;
50       Z11 += m11;
51       Z21 += m21;
52       Z31 += m31;
53       Z41 += m41;
54       /* load p and q values */
55       p1=ell[0];
56       q1=ex[-1];
57       p2=ell[-1];
58       p3=ell[-2];
59       p4=ell[-3];
60       /* compute outer product and add it to the Z matrix */
61       m11 = p1 * q1;
62       m21 = p2 * q1;
63       m31 = p3 * q1;
64       m41 = p4 * q1;
65       ell += lskip1;
66       Z11 += m11;
67       Z21 += m21;
68       Z31 += m31;
69       Z41 += m41;
70       /* load p and q values */
71       p1=ell[0];
72       q1=ex[-2];
73       p2=ell[-1];
74       p3=ell[-2];
75       p4=ell[-3];
76       /* compute outer product and add it to the Z matrix */
77       m11 = p1 * q1;
78       m21 = p2 * q1;
79       m31 = p3 * q1;
80       m41 = p4 * q1;
81       ell += lskip1;
82       Z11 += m11;
83       Z21 += m21;
84       Z31 += m31;
85       Z41 += m41;
86       /* load p and q values */
87       p1=ell[0];
88       q1=ex[-3];
89       p2=ell[-1];
90       p3=ell[-2];
91       p4=ell[-3];
92       /* compute outer product and add it to the Z matrix */
93       m11 = p1 * q1;
94       m21 = p2 * q1;
95       m31 = p3 * q1;
96       m41 = p4 * q1;
97       ell += lskip1;
98       ex -= 4;
99       Z11 += m11;
100       Z21 += m21;
101       Z31 += m31;
102       Z41 += m41;
103       /* end of inner loop */
104     }
105     /* compute left-over iterations */
106     j += 4;
107     for (; j > 0; j--) {
108       /* load p and q values */
109       p1=ell[0];
110       q1=ex[0];
111       p2=ell[-1];
112       p3=ell[-2];
113       p4=ell[-3];
114       /* compute outer product and add it to the Z matrix */
115       m11 = p1 * q1;
116       m21 = p2 * q1;
117       m31 = p3 * q1;
118       m41 = p4 * q1;
119       ell += lskip1;
120       ex -= 1;
121       Z11 += m11;
122       Z21 += m21;
123       Z31 += m31;
124       Z41 += m41;
125     }
126     /* finish computing the X(i) block */
127     Z11 = ex[0] - Z11;
128     ex[0] = Z11;
129     p1 = ell[-1];
130     Z21 = ex[-1] - Z21 - p1*Z11;
131     ex[-1] = Z21;
132     p1 = ell[-2];
133     p2 = ell[-2+lskip1];
134     Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
135     ex[-2] = Z31;
136     p1 = ell[-3];
137     p2 = ell[-3+lskip1];
138     p3 = ell[-3+lskip2];
139     Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
140     ex[-3] = Z41;
141     /* end of outer loop */
142   }
143   /* compute rows at end that are not a multiple of block size */
144   for (; i < n; i++) {
145     /* compute all 1 x 1 block of X, from rows i..i+1-1 */
146     /* set the Z matrix to 0 */
147     Z11=0;
148     ell = L - i;
149     ex = B;
150     /* the inner loop that computes outer products and adds them to Z */
151     for (j=i-4; j >= 0; j -= 4) {
152       /* load p and q values */
153       p1=ell[0];
154       q1=ex[0];
155       /* compute outer product and add it to the Z matrix */
156       m11 = p1 * q1;
157       ell += lskip1;
158       Z11 += m11;
159       /* load p and q values */
160       p1=ell[0];
161       q1=ex[-1];
162       /* compute outer product and add it to the Z matrix */
163       m11 = p1 * q1;
164       ell += lskip1;
165       Z11 += m11;
166       /* load p and q values */
167       p1=ell[0];
168       q1=ex[-2];
169       /* compute outer product and add it to the Z matrix */
170       m11 = p1 * q1;
171       ell += lskip1;
172       Z11 += m11;
173       /* load p and q values */
174       p1=ell[0];
175       q1=ex[-3];
176       /* compute outer product and add it to the Z matrix */
177       m11 = p1 * q1;
178       ell += lskip1;
179       ex -= 4;
180       Z11 += m11;
181       /* end of inner loop */
182     }
183     /* compute left-over iterations */
184     j += 4;
185     for (; j > 0; j--) {
186       /* load p and q values */
187       p1=ell[0];
188       q1=ex[0];
189       /* compute outer product and add it to the Z matrix */
190       m11 = p1 * q1;
191       ell += lskip1;
192       ex -= 1;
193       Z11 += m11;
194     }
195     /* finish computing the X(i) block */
196     Z11 = ex[0] - Z11;
197     ex[0] = Z11;
198   }
199 }