1 /*
2  * Copyright (C) 2011 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 /* $Id: db_utilities_linalg.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */
18 
19 #include "db_utilities_linalg.h"
20 #include "db_utilities.h"
21 
22 
23 
24 /*****************************************************************
25 *    Lean and mean begins here                                   *
26 *****************************************************************/
27 
28 /*Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper
29 part of A is used from the input. The Cholesky factor is output as
30 subdiagonal part in A and diagonal in d, which is 6-dimensional*/
db_CholeskyDecomp6x6(double A[36],double d[6])31 void db_CholeskyDecomp6x6(double A[36],double d[6])
32 {
33     double s,temp;
34 
35     /*[50 mult 35 add 6sqrt=85flops 6func]*/
36     /*i=0*/
37     s=A[0];
38     d[0]=((s>0.0)?sqrt(s):1.0);
39     temp=db_SafeReciprocal(d[0]);
40     A[6]=A[1]*temp;
41     A[12]=A[2]*temp;
42     A[18]=A[3]*temp;
43     A[24]=A[4]*temp;
44     A[30]=A[5]*temp;
45     /*i=1*/
46     s=A[7]-A[6]*A[6];
47     d[1]=((s>0.0)?sqrt(s):1.0);
48     temp=db_SafeReciprocal(d[1]);
49     A[13]=(A[8]-A[6]*A[12])*temp;
50     A[19]=(A[9]-A[6]*A[18])*temp;
51     A[25]=(A[10]-A[6]*A[24])*temp;
52     A[31]=(A[11]-A[6]*A[30])*temp;
53     /*i=2*/
54     s=A[14]-A[12]*A[12]-A[13]*A[13];
55     d[2]=((s>0.0)?sqrt(s):1.0);
56     temp=db_SafeReciprocal(d[2]);
57     A[20]=(A[15]-A[12]*A[18]-A[13]*A[19])*temp;
58     A[26]=(A[16]-A[12]*A[24]-A[13]*A[25])*temp;
59     A[32]=(A[17]-A[12]*A[30]-A[13]*A[31])*temp;
60     /*i=3*/
61     s=A[21]-A[18]*A[18]-A[19]*A[19]-A[20]*A[20];
62     d[3]=((s>0.0)?sqrt(s):1.0);
63     temp=db_SafeReciprocal(d[3]);
64     A[27]=(A[22]-A[18]*A[24]-A[19]*A[25]-A[20]*A[26])*temp;
65     A[33]=(A[23]-A[18]*A[30]-A[19]*A[31]-A[20]*A[32])*temp;
66     /*i=4*/
67     s=A[28]-A[24]*A[24]-A[25]*A[25]-A[26]*A[26]-A[27]*A[27];
68     d[4]=((s>0.0)?sqrt(s):1.0);
69     temp=db_SafeReciprocal(d[4]);
70     A[34]=(A[29]-A[24]*A[30]-A[25]*A[31]-A[26]*A[32]-A[27]*A[33])*temp;
71     /*i=5*/
72     s=A[35]-A[30]*A[30]-A[31]*A[31]-A[32]*A[32]-A[33]*A[33]-A[34]*A[34];
73     d[5]=((s>0.0)?sqrt(s):1.0);
74 }
75 
76 /*Cholesky-factorize symmetric positive definite n x n matrix A.Part
77 above diagonal of A is used from the input, diagonal of A is assumed to
78 be stored in d. The Cholesky factor is output as
79 subdiagonal part in A and diagonal in d, which is n-dimensional*/
db_CholeskyDecompSeparateDiagonal(double ** A,double * d,int n)80 void db_CholeskyDecompSeparateDiagonal(double **A,double *d,int n)
81 {
82     int i,j,k;
83     double s;
84     double temp = 0.0;
85 
86     for(i=0;i<n;i++) for(j=i;j<n;j++)
87     {
88         if(i==j) s=d[i];
89         else s=A[i][j];
90         for(k=i-1;k>=0;k--) s-=A[i][k]*A[j][k];
91         if(i==j)
92         {
93             d[i]=((s>0.0)?sqrt(s):1.0);
94             temp=db_SafeReciprocal(d[i]);
95         }
96         else A[j][i]=s*temp;
97     }
98 }
99 
100 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
101 of an n x n matrix and the right hand side b. The vector b is unchanged*/
db_CholeskyBacksub(double * x,const double * const * A,const double * d,int n,const double * b)102 void db_CholeskyBacksub(double *x,const double * const *A,const double *d,int n,const double *b)
103 {
104     int i,k;
105     double s;
106 
107     for(i=0;i<n;i++)
108     {
109         for(s=b[i],k=i-1;k>=0;k--) s-=A[i][k]*x[k];
110         x[i]=db_SafeDivision(s,d[i]);
111     }
112     for(i=n-1;i>=0;i--)
113     {
114         for(s=x[i],k=i+1;k<n;k++) s-=A[k][i]*x[k];
115         x[i]=db_SafeDivision(s,d[i]);
116     }
117 }
118 
119 /*Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part
120 above diagonal of A is used from the input, diagonal of A is assumed to
121 be stored in d. The Cholesky factor is output as subdiagonal part in A
122 and diagonal in d, which is 3-dimensional*/
db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3])123 void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3])
124 {
125     double s,temp;
126 
127     /*i=0*/
128     s=d[0];
129     d[0]=((s>0.0)?sqrt(s):1.0);
130     temp=db_SafeReciprocal(d[0]);
131     A[3]=A[1]*temp;
132     A[6]=A[2]*temp;
133     /*i=1*/
134     s=d[1]-A[3]*A[3];
135     d[1]=((s>0.0)?sqrt(s):1.0);
136     temp=db_SafeReciprocal(d[1]);
137     A[7]=(A[5]-A[3]*A[6])*temp;
138     /*i=2*/
139     s=d[2]-A[6]*A[6]-A[7]*A[7];
140     d[2]=((s>0.0)?sqrt(s):1.0);
141 }
142 
143 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
144 of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/
db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3])145 void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3])
146 {
147     /*[42 mult 30 add=72flops]*/
148     x[0]=db_SafeDivision(b[0],d[0]);
149     x[1]=db_SafeDivision((b[1]-A[3]*x[0]),d[1]);
150     x[2]=db_SafeDivision((b[2]-A[6]*x[0]-A[7]*x[1]),d[2]);
151     x[2]=db_SafeDivision(x[2],d[2]);
152     x[1]=db_SafeDivision((x[1]-A[7]*x[2]),d[1]);
153     x[0]=db_SafeDivision((x[0]-A[6]*x[2]-A[3]*x[1]),d[0]);
154 }
155 
156 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
157 of a 6 x 6 matrix and the right hand side b. The vector b is unchanged*/
db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6])158 void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6])
159 {
160     /*[42 mult 30 add=72flops]*/
161     x[0]=db_SafeDivision(b[0],d[0]);
162     x[1]=db_SafeDivision((b[1]-A[6]*x[0]),d[1]);
163     x[2]=db_SafeDivision((b[2]-A[12]*x[0]-A[13]*x[1]),d[2]);
164     x[3]=db_SafeDivision((b[3]-A[18]*x[0]-A[19]*x[1]-A[20]*x[2]),d[3]);
165     x[4]=db_SafeDivision((b[4]-A[24]*x[0]-A[25]*x[1]-A[26]*x[2]-A[27]*x[3]),d[4]);
166     x[5]=db_SafeDivision((b[5]-A[30]*x[0]-A[31]*x[1]-A[32]*x[2]-A[33]*x[3]-A[34]*x[4]),d[5]);
167     x[5]=db_SafeDivision(x[5],d[5]);
168     x[4]=db_SafeDivision((x[4]-A[34]*x[5]),d[4]);
169     x[3]=db_SafeDivision((x[3]-A[33]*x[5]-A[27]*x[4]),d[3]);
170     x[2]=db_SafeDivision((x[2]-A[32]*x[5]-A[26]*x[4]-A[20]*x[3]),d[2]);
171     x[1]=db_SafeDivision((x[1]-A[31]*x[5]-A[25]*x[4]-A[19]*x[3]-A[13]*x[2]),d[1]);
172     x[0]=db_SafeDivision((x[0]-A[30]*x[5]-A[24]*x[4]-A[18]*x[3]-A[12]*x[2]-A[6]*x[1]),d[0]);
173 }
174 
175 
db_Orthogonalize6x7(double A[42],int orthonormalize)176 void db_Orthogonalize6x7(double A[42],int orthonormalize)
177 {
178     int i;
179     double ss[6];
180 
181     /*Compute square sums of rows*/
182     ss[0]=db_SquareSum7(A);
183     ss[1]=db_SquareSum7(A+7);
184     ss[2]=db_SquareSum7(A+14);
185     ss[3]=db_SquareSum7(A+21);
186     ss[4]=db_SquareSum7(A+28);
187     ss[5]=db_SquareSum7(A+35);
188 
189     ss[1]-=db_OrthogonalizePair7(A+7 ,A,ss[0]);
190     ss[2]-=db_OrthogonalizePair7(A+14,A,ss[0]);
191     ss[3]-=db_OrthogonalizePair7(A+21,A,ss[0]);
192     ss[4]-=db_OrthogonalizePair7(A+28,A,ss[0]);
193     ss[5]-=db_OrthogonalizePair7(A+35,A,ss[0]);
194 
195     /*Pivot on largest ss (could also be done on ss/(original_ss))*/
196     i=db_MaxIndex5(ss+1);
197     db_OrthogonalizationSwap7(A+7,i,ss+1);
198 
199     ss[2]-=db_OrthogonalizePair7(A+14,A+7,ss[1]);
200     ss[3]-=db_OrthogonalizePair7(A+21,A+7,ss[1]);
201     ss[4]-=db_OrthogonalizePair7(A+28,A+7,ss[1]);
202     ss[5]-=db_OrthogonalizePair7(A+35,A+7,ss[1]);
203 
204     i=db_MaxIndex4(ss+2);
205     db_OrthogonalizationSwap7(A+14,i,ss+2);
206 
207     ss[3]-=db_OrthogonalizePair7(A+21,A+14,ss[2]);
208     ss[4]-=db_OrthogonalizePair7(A+28,A+14,ss[2]);
209     ss[5]-=db_OrthogonalizePair7(A+35,A+14,ss[2]);
210 
211     i=db_MaxIndex3(ss+3);
212     db_OrthogonalizationSwap7(A+21,i,ss+3);
213 
214     ss[4]-=db_OrthogonalizePair7(A+28,A+21,ss[3]);
215     ss[5]-=db_OrthogonalizePair7(A+35,A+21,ss[3]);
216 
217     i=db_MaxIndex2(ss+4);
218     db_OrthogonalizationSwap7(A+28,i,ss+4);
219 
220     ss[5]-=db_OrthogonalizePair7(A+35,A+28,ss[4]);
221 
222     if(orthonormalize)
223     {
224         db_MultiplyScalar7(A   ,db_SafeSqrtReciprocal(ss[0]));
225         db_MultiplyScalar7(A+7 ,db_SafeSqrtReciprocal(ss[1]));
226         db_MultiplyScalar7(A+14,db_SafeSqrtReciprocal(ss[2]));
227         db_MultiplyScalar7(A+21,db_SafeSqrtReciprocal(ss[3]));
228         db_MultiplyScalar7(A+28,db_SafeSqrtReciprocal(ss[4]));
229         db_MultiplyScalar7(A+35,db_SafeSqrtReciprocal(ss[5]));
230     }
231 }
232 
db_Orthogonalize8x9(double A[72],int orthonormalize)233 void db_Orthogonalize8x9(double A[72],int orthonormalize)
234 {
235     int i;
236     double ss[8];
237 
238     /*Compute square sums of rows*/
239     ss[0]=db_SquareSum9(A);
240     ss[1]=db_SquareSum9(A+9);
241     ss[2]=db_SquareSum9(A+18);
242     ss[3]=db_SquareSum9(A+27);
243     ss[4]=db_SquareSum9(A+36);
244     ss[5]=db_SquareSum9(A+45);
245     ss[6]=db_SquareSum9(A+54);
246     ss[7]=db_SquareSum9(A+63);
247 
248     ss[1]-=db_OrthogonalizePair9(A+9 ,A,ss[0]);
249     ss[2]-=db_OrthogonalizePair9(A+18,A,ss[0]);
250     ss[3]-=db_OrthogonalizePair9(A+27,A,ss[0]);
251     ss[4]-=db_OrthogonalizePair9(A+36,A,ss[0]);
252     ss[5]-=db_OrthogonalizePair9(A+45,A,ss[0]);
253     ss[6]-=db_OrthogonalizePair9(A+54,A,ss[0]);
254     ss[7]-=db_OrthogonalizePair9(A+63,A,ss[0]);
255 
256     /*Pivot on largest ss (could also be done on ss/(original_ss))*/
257     i=db_MaxIndex7(ss+1);
258     db_OrthogonalizationSwap9(A+9,i,ss+1);
259 
260     ss[2]-=db_OrthogonalizePair9(A+18,A+9,ss[1]);
261     ss[3]-=db_OrthogonalizePair9(A+27,A+9,ss[1]);
262     ss[4]-=db_OrthogonalizePair9(A+36,A+9,ss[1]);
263     ss[5]-=db_OrthogonalizePair9(A+45,A+9,ss[1]);
264     ss[6]-=db_OrthogonalizePair9(A+54,A+9,ss[1]);
265     ss[7]-=db_OrthogonalizePair9(A+63,A+9,ss[1]);
266 
267     i=db_MaxIndex6(ss+2);
268     db_OrthogonalizationSwap9(A+18,i,ss+2);
269 
270     ss[3]-=db_OrthogonalizePair9(A+27,A+18,ss[2]);
271     ss[4]-=db_OrthogonalizePair9(A+36,A+18,ss[2]);
272     ss[5]-=db_OrthogonalizePair9(A+45,A+18,ss[2]);
273     ss[6]-=db_OrthogonalizePair9(A+54,A+18,ss[2]);
274     ss[7]-=db_OrthogonalizePair9(A+63,A+18,ss[2]);
275 
276     i=db_MaxIndex5(ss+3);
277     db_OrthogonalizationSwap9(A+27,i,ss+3);
278 
279     ss[4]-=db_OrthogonalizePair9(A+36,A+27,ss[3]);
280     ss[5]-=db_OrthogonalizePair9(A+45,A+27,ss[3]);
281     ss[6]-=db_OrthogonalizePair9(A+54,A+27,ss[3]);
282     ss[7]-=db_OrthogonalizePair9(A+63,A+27,ss[3]);
283 
284     i=db_MaxIndex4(ss+4);
285     db_OrthogonalizationSwap9(A+36,i,ss+4);
286 
287     ss[5]-=db_OrthogonalizePair9(A+45,A+36,ss[4]);
288     ss[6]-=db_OrthogonalizePair9(A+54,A+36,ss[4]);
289     ss[7]-=db_OrthogonalizePair9(A+63,A+36,ss[4]);
290 
291     i=db_MaxIndex3(ss+5);
292     db_OrthogonalizationSwap9(A+45,i,ss+5);
293 
294     ss[6]-=db_OrthogonalizePair9(A+54,A+45,ss[5]);
295     ss[7]-=db_OrthogonalizePair9(A+63,A+45,ss[5]);
296 
297     i=db_MaxIndex2(ss+6);
298     db_OrthogonalizationSwap9(A+54,i,ss+6);
299 
300     ss[7]-=db_OrthogonalizePair9(A+63,A+54,ss[6]);
301 
302     if(orthonormalize)
303     {
304         db_MultiplyScalar9(A   ,db_SafeSqrtReciprocal(ss[0]));
305         db_MultiplyScalar9(A+9 ,db_SafeSqrtReciprocal(ss[1]));
306         db_MultiplyScalar9(A+18,db_SafeSqrtReciprocal(ss[2]));
307         db_MultiplyScalar9(A+27,db_SafeSqrtReciprocal(ss[3]));
308         db_MultiplyScalar9(A+36,db_SafeSqrtReciprocal(ss[4]));
309         db_MultiplyScalar9(A+45,db_SafeSqrtReciprocal(ss[5]));
310         db_MultiplyScalar9(A+54,db_SafeSqrtReciprocal(ss[6]));
311         db_MultiplyScalar9(A+63,db_SafeSqrtReciprocal(ss[7]));
312     }
313 }
314 
db_NullVectorOrthonormal6x7(double x[7],const double A[42])315 void db_NullVectorOrthonormal6x7(double x[7],const double A[42])
316 {
317     int i;
318     double omss[7];
319     const double *B;
320 
321     /*Pivot by choosing row of the identity matrix
322     (the one corresponding to column of A with smallest square sum)*/
323     omss[0]=db_SquareSum6Stride7(A);
324     omss[1]=db_SquareSum6Stride7(A+1);
325     omss[2]=db_SquareSum6Stride7(A+2);
326     omss[3]=db_SquareSum6Stride7(A+3);
327     omss[4]=db_SquareSum6Stride7(A+4);
328     omss[5]=db_SquareSum6Stride7(A+5);
329     omss[6]=db_SquareSum6Stride7(A+6);
330     i=db_MinIndex7(omss);
331     /*orthogonalize that row against all previous rows
332     and normalize it*/
333     B=A+i;
334     db_MultiplyScalarCopy7(x,A,-B[0]);
335     db_RowOperation7(x,A+7 ,B[7]);
336     db_RowOperation7(x,A+14,B[14]);
337     db_RowOperation7(x,A+21,B[21]);
338     db_RowOperation7(x,A+28,B[28]);
339     db_RowOperation7(x,A+35,B[35]);
340     x[i]+=1.0;
341     db_MultiplyScalar7(x,db_SafeSqrtReciprocal(1.0-omss[i]));
342 }
343 
db_NullVectorOrthonormal8x9(double x[9],const double A[72])344 void db_NullVectorOrthonormal8x9(double x[9],const double A[72])
345 {
346     int i;
347     double omss[9];
348     const double *B;
349 
350     /*Pivot by choosing row of the identity matrix
351     (the one corresponding to column of A with smallest square sum)*/
352     omss[0]=db_SquareSum8Stride9(A);
353     omss[1]=db_SquareSum8Stride9(A+1);
354     omss[2]=db_SquareSum8Stride9(A+2);
355     omss[3]=db_SquareSum8Stride9(A+3);
356     omss[4]=db_SquareSum8Stride9(A+4);
357     omss[5]=db_SquareSum8Stride9(A+5);
358     omss[6]=db_SquareSum8Stride9(A+6);
359     omss[7]=db_SquareSum8Stride9(A+7);
360     omss[8]=db_SquareSum8Stride9(A+8);
361     i=db_MinIndex9(omss);
362     /*orthogonalize that row against all previous rows
363     and normalize it*/
364     B=A+i;
365     db_MultiplyScalarCopy9(x,A,-B[0]);
366     db_RowOperation9(x,A+9 ,B[9]);
367     db_RowOperation9(x,A+18,B[18]);
368     db_RowOperation9(x,A+27,B[27]);
369     db_RowOperation9(x,A+36,B[36]);
370     db_RowOperation9(x,A+45,B[45]);
371     db_RowOperation9(x,A+54,B[54]);
372     db_RowOperation9(x,A+63,B[63]);
373     x[i]+=1.0;
374     db_MultiplyScalar9(x,db_SafeSqrtReciprocal(1.0-omss[i]));
375 }
376 
377