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*/ 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*/ 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*/ 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*/ 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*/ 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*/ 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 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 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 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 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
