perftests/panorama/feature_stab/db_vlvm/db_utilities_linalg.cpp - platform/development - Git at Google

 /*
  * Copyright (C) 2011 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 /* $Id: db_utilities_linalg.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */

 #include "db_utilities_linalg.h"
 #include "db_utilities.h"


 /*****************************************************************
 *    Lean and mean begins here                                   *
 *****************************************************************/

 /*Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper
 part of A is used from the input. The Cholesky factor is output as
 subdiagonal part in A and diagonal in d, which is 6-dimensional*/
 void db_CholeskyDecomp6x6(double A[36],double d[6])
 {
     double s,temp;

     /*[50 mult 35 add 6sqrt=85flops 6func]*/
     /*i=0*/
     s=A[0];
     d[0]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[0]);
     A[6]=A[1]*temp;
     A[12]=A[2]*temp;
     A[18]=A[3]*temp;
     A[24]=A[4]*temp;
     A[30]=A[5]*temp;
     /*i=1*/
     s=A[7]-A[6]*A[6];
     d[1]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[1]);
     A[13]=(A[8]-A[6]*A[12])*temp;
     A[19]=(A[9]-A[6]*A[18])*temp;
     A[25]=(A[10]-A[6]*A[24])*temp;
     A[31]=(A[11]-A[6]*A[30])*temp;
     /*i=2*/
     s=A[14]-A[12]*A[12]-A[13]*A[13];
     d[2]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[2]);
     A[20]=(A[15]-A[12]*A[18]-A[13]*A[19])*temp;
     A[26]=(A[16]-A[12]*A[24]-A[13]*A[25])*temp;
     A[32]=(A[17]-A[12]*A[30]-A[13]*A[31])*temp;
     /*i=3*/
     s=A[21]-A[18]*A[18]-A[19]*A[19]-A[20]*A[20];
     d[3]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[3]);
     A[27]=(A[22]-A[18]*A[24]-A[19]*A[25]-A[20]*A[26])*temp;
     A[33]=(A[23]-A[18]*A[30]-A[19]*A[31]-A[20]*A[32])*temp;
     /*i=4*/
     s=A[28]-A[24]*A[24]-A[25]*A[25]-A[26]*A[26]-A[27]*A[27];
     d[4]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[4]);
     A[34]=(A[29]-A[24]*A[30]-A[25]*A[31]-A[26]*A[32]-A[27]*A[33])*temp;
     /*i=5*/
     s=A[35]-A[30]*A[30]-A[31]*A[31]-A[32]*A[32]-A[33]*A[33]-A[34]*A[34];
     d[5]=((s>0.0)?sqrt(s):1.0);
 }

 /*Cholesky-factorize symmetric positive definite n x n matrix A.Part
 above diagonal of A is used from the input, diagonal of A is assumed to
 be stored in d. The Cholesky factor is output as
 subdiagonal part in A and diagonal in d, which is n-dimensional*/
 void db_CholeskyDecompSeparateDiagonal(double **A,double *d,int n)
 {
     int i,j,k;
     double s;
     double temp = 0.0;

     for(i=0;i<n;i++) for(j=i;j<n;j++)
     {
         if(i==j) s=d[i];
         else s=A[i][j];
         for(k=i-1;k>=0;k--) s-=A[i][k]*A[j][k];
         if(i==j)
         {
             d[i]=((s>0.0)?sqrt(s):1.0);
             temp=db_SafeReciprocal(d[i]);
         }
         else A[j][i]=s*temp;
     }
 }

 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
 of an n x n matrix and the right hand side b. The vector b is unchanged*/
 void db_CholeskyBacksub(double *x,const double * const *A,const double *d,int n,const double *b)
 {
     int i,k;
     double s;

     for(i=0;i<n;i++)
     {
         for(s=b[i],k=i-1;k>=0;k--) s-=A[i][k]*x[k];
         x[i]=db_SafeDivision(s,d[i]);
     }
     for(i=n-1;i>=0;i--)
     {
         for(s=x[i],k=i+1;k<n;k++) s-=A[k][i]*x[k];
         x[i]=db_SafeDivision(s,d[i]);
     }
 }

 /*Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part
 above diagonal of A is used from the input, diagonal of A is assumed to
 be stored in d. The Cholesky factor is output as subdiagonal part in A
 and diagonal in d, which is 3-dimensional*/
 void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3])
 {
     double s,temp;

     /*i=0*/
     s=d[0];
     d[0]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[0]);
     A[3]=A[1]*temp;
     A[6]=A[2]*temp;
     /*i=1*/
     s=d[1]-A[3]*A[3];
     d[1]=((s>0.0)?sqrt(s):1.0);
     temp=db_SafeReciprocal(d[1]);
     A[7]=(A[5]-A[3]*A[6])*temp;
     /*i=2*/
     s=d[2]-A[6]*A[6]-A[7]*A[7];
     d[2]=((s>0.0)?sqrt(s):1.0);
 }

 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
 of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/
 void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3])
 {
     /*[42 mult 30 add=72flops]*/
     x[0]=db_SafeDivision(b[0],d[0]);
     x[1]=db_SafeDivision((b[1]-A[3]*x[0]),d[1]);
     x[2]=db_SafeDivision((b[2]-A[6]*x[0]-A[7]*x[1]),d[2]);
     x[2]=db_SafeDivision(x[2],d[2]);
     x[1]=db_SafeDivision((x[1]-A[7]*x[2]),d[1]);
     x[0]=db_SafeDivision((x[0]-A[6]*x[2]-A[3]*x[1]),d[0]);
 }

 /*Backsubstitute L%transpose(L)*x=b for x given the Cholesky decomposition
 of a 6 x 6 matrix and the right hand side b. The vector b is unchanged*/
 void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6])
 {
     /*[42 mult 30 add=72flops]*/
     x[0]=db_SafeDivision(b[0],d[0]);
     x[1]=db_SafeDivision((b[1]-A[6]*x[0]),d[1]);
     x[2]=db_SafeDivision((b[2]-A[12]*x[0]-A[13]*x[1]),d[2]);
     x[3]=db_SafeDivision((b[3]-A[18]*x[0]-A[19]*x[1]-A[20]*x[2]),d[3]);
     x[4]=db_SafeDivision((b[4]-A[24]*x[0]-A[25]*x[1]-A[26]*x[2]-A[27]*x[3]),d[4]);
     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]);
     x[5]=db_SafeDivision(x[5],d[5]);
     x[4]=db_SafeDivision((x[4]-A[34]*x[5]),d[4]);
     x[3]=db_SafeDivision((x[3]-A[33]*x[5]-A[27]*x[4]),d[3]);
     x[2]=db_SafeDivision((x[2]-A[32]*x[5]-A[26]*x[4]-A[20]*x[3]),d[2]);
     x[1]=db_SafeDivision((x[1]-A[31]*x[5]-A[25]*x[4]-A[19]*x[3]-A[13]*x[2]),d[1]);
     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]);
 }


 void db_Orthogonalize6x7(double A[42],int orthonormalize)
 {
     int i;
     double ss[6];

     /*Compute square sums of rows*/
     ss[0]=db_SquareSum7(A);
     ss[1]=db_SquareSum7(A+7);
     ss[2]=db_SquareSum7(A+14);
     ss[3]=db_SquareSum7(A+21);
     ss[4]=db_SquareSum7(A+28);
     ss[5]=db_SquareSum7(A+35);

     ss[1]-=db_OrthogonalizePair7(A+7 ,A,ss[0]);
     ss[2]-=db_OrthogonalizePair7(A+14,A,ss[0]);
     ss[3]-=db_OrthogonalizePair7(A+21,A,ss[0]);
     ss[4]-=db_OrthogonalizePair7(A+28,A,ss[0]);
     ss[5]-=db_OrthogonalizePair7(A+35,A,ss[0]);

     /*Pivot on largest ss (could also be done on ss/(original_ss))*/
     i=db_MaxIndex5(ss+1);
     db_OrthogonalizationSwap7(A+7,i,ss+1);

     ss[2]-=db_OrthogonalizePair7(A+14,A+7,ss[1]);
     ss[3]-=db_OrthogonalizePair7(A+21,A+7,ss[1]);
     ss[4]-=db_OrthogonalizePair7(A+28,A+7,ss[1]);
     ss[5]-=db_OrthogonalizePair7(A+35,A+7,ss[1]);

     i=db_MaxIndex4(ss+2);
     db_OrthogonalizationSwap7(A+14,i,ss+2);

     ss[3]-=db_OrthogonalizePair7(A+21,A+14,ss[2]);
     ss[4]-=db_OrthogonalizePair7(A+28,A+14,ss[2]);
     ss[5]-=db_OrthogonalizePair7(A+35,A+14,ss[2]);

     i=db_MaxIndex3(ss+3);
     db_OrthogonalizationSwap7(A+21,i,ss+3);

     ss[4]-=db_OrthogonalizePair7(A+28,A+21,ss[3]);
     ss[5]-=db_OrthogonalizePair7(A+35,A+21,ss[3]);

     i=db_MaxIndex2(ss+4);
     db_OrthogonalizationSwap7(A+28,i,ss+4);

     ss[5]-=db_OrthogonalizePair7(A+35,A+28,ss[4]);

     if(orthonormalize)
     {
         db_MultiplyScalar7(A   ,db_SafeSqrtReciprocal(ss[0]));
         db_MultiplyScalar7(A+7 ,db_SafeSqrtReciprocal(ss[1]));
         db_MultiplyScalar7(A+14,db_SafeSqrtReciprocal(ss[2]));
         db_MultiplyScalar7(A+21,db_SafeSqrtReciprocal(ss[3]));
         db_MultiplyScalar7(A+28,db_SafeSqrtReciprocal(ss[4]));
         db_MultiplyScalar7(A+35,db_SafeSqrtReciprocal(ss[5]));
     }
 }

 void db_Orthogonalize8x9(double A[72],int orthonormalize)
 {
     int i;
     double ss[8];

     /*Compute square sums of rows*/
     ss[0]=db_SquareSum9(A);
     ss[1]=db_SquareSum9(A+9);
     ss[2]=db_SquareSum9(A+18);
     ss[3]=db_SquareSum9(A+27);
     ss[4]=db_SquareSum9(A+36);
     ss[5]=db_SquareSum9(A+45);
     ss[6]=db_SquareSum9(A+54);
     ss[7]=db_SquareSum9(A+63);

     ss[1]-=db_OrthogonalizePair9(A+9 ,A,ss[0]);
     ss[2]-=db_OrthogonalizePair9(A+18,A,ss[0]);
     ss[3]-=db_OrthogonalizePair9(A+27,A,ss[0]);
     ss[4]-=db_OrthogonalizePair9(A+36,A,ss[0]);
     ss[5]-=db_OrthogonalizePair9(A+45,A,ss[0]);
     ss[6]-=db_OrthogonalizePair9(A+54,A,ss[0]);
     ss[7]-=db_OrthogonalizePair9(A+63,A,ss[0]);

     /*Pivot on largest ss (could also be done on ss/(original_ss))*/
     i=db_MaxIndex7(ss+1);
     db_OrthogonalizationSwap9(A+9,i,ss+1);

     ss[2]-=db_OrthogonalizePair9(A+18,A+9,ss[1]);
     ss[3]-=db_OrthogonalizePair9(A+27,A+9,ss[1]);
     ss[4]-=db_OrthogonalizePair9(A+36,A+9,ss[1]);
     ss[5]-=db_OrthogonalizePair9(A+45,A+9,ss[1]);
     ss[6]-=db_OrthogonalizePair9(A+54,A+9,ss[1]);
     ss[7]-=db_OrthogonalizePair9(A+63,A+9,ss[1]);

     i=db_MaxIndex6(ss+2);
     db_OrthogonalizationSwap9(A+18,i,ss+2);

     ss[3]-=db_OrthogonalizePair9(A+27,A+18,ss[2]);
     ss[4]-=db_OrthogonalizePair9(A+36,A+18,ss[2]);
     ss[5]-=db_OrthogonalizePair9(A+45,A+18,ss[2]);
     ss[6]-=db_OrthogonalizePair9(A+54,A+18,ss[2]);
     ss[7]-=db_OrthogonalizePair9(A+63,A+18,ss[2]);

     i=db_MaxIndex5(ss+3);
     db_OrthogonalizationSwap9(A+27,i,ss+3);

     ss[4]-=db_OrthogonalizePair9(A+36,A+27,ss[3]);
     ss[5]-=db_OrthogonalizePair9(A+45,A+27,ss[3]);
     ss[6]-=db_OrthogonalizePair9(A+54,A+27,ss[3]);
     ss[7]-=db_OrthogonalizePair9(A+63,A+27,ss[3]);

     i=db_MaxIndex4(ss+4);
     db_OrthogonalizationSwap9(A+36,i,ss+4);

     ss[5]-=db_OrthogonalizePair9(A+45,A+36,ss[4]);
     ss[6]-=db_OrthogonalizePair9(A+54,A+36,ss[4]);
     ss[7]-=db_OrthogonalizePair9(A+63,A+36,ss[4]);

     i=db_MaxIndex3(ss+5);
     db_OrthogonalizationSwap9(A+45,i,ss+5);

     ss[6]-=db_OrthogonalizePair9(A+54,A+45,ss[5]);
     ss[7]-=db_OrthogonalizePair9(A+63,A+45,ss[5]);

     i=db_MaxIndex2(ss+6);
     db_OrthogonalizationSwap9(A+54,i,ss+6);

     ss[7]-=db_OrthogonalizePair9(A+63,A+54,ss[6]);

     if(orthonormalize)
     {
         db_MultiplyScalar9(A   ,db_SafeSqrtReciprocal(ss[0]));
         db_MultiplyScalar9(A+9 ,db_SafeSqrtReciprocal(ss[1]));
         db_MultiplyScalar9(A+18,db_SafeSqrtReciprocal(ss[2]));
         db_MultiplyScalar9(A+27,db_SafeSqrtReciprocal(ss[3]));
         db_MultiplyScalar9(A+36,db_SafeSqrtReciprocal(ss[4]));
         db_MultiplyScalar9(A+45,db_SafeSqrtReciprocal(ss[5]));
         db_MultiplyScalar9(A+54,db_SafeSqrtReciprocal(ss[6]));
         db_MultiplyScalar9(A+63,db_SafeSqrtReciprocal(ss[7]));
     }
 }

 void db_NullVectorOrthonormal6x7(double x[7],const double A[42])
 {
     int i;
     double omss[7];
     const double *B;

     /*Pivot by choosing row of the identity matrix
     (the one corresponding to column of A with smallest square sum)*/
     omss[0]=db_SquareSum6Stride7(A);
     omss[1]=db_SquareSum6Stride7(A+1);
     omss[2]=db_SquareSum6Stride7(A+2);
     omss[3]=db_SquareSum6Stride7(A+3);
     omss[4]=db_SquareSum6Stride7(A+4);
     omss[5]=db_SquareSum6Stride7(A+5);
     omss[6]=db_SquareSum6Stride7(A+6);
     i=db_MinIndex7(omss);
     /*orthogonalize that row against all previous rows
     and normalize it*/
     B=A+i;
     db_MultiplyScalarCopy7(x,A,-B[0]);
     db_RowOperation7(x,A+7 ,B[7]);
     db_RowOperation7(x,A+14,B[14]);
     db_RowOperation7(x,A+21,B[21]);
     db_RowOperation7(x,A+28,B[28]);
     db_RowOperation7(x,A+35,B[35]);
     x[i]+=1.0;
     db_MultiplyScalar7(x,db_SafeSqrtReciprocal(1.0-omss[i]));
 }

 void db_NullVectorOrthonormal8x9(double x[9],const double A[72])
 {
     int i;
     double omss[9];
     const double *B;

     /*Pivot by choosing row of the identity matrix
     (the one corresponding to column of A with smallest square sum)*/
     omss[0]=db_SquareSum8Stride9(A);
     omss[1]=db_SquareSum8Stride9(A+1);
     omss[2]=db_SquareSum8Stride9(A+2);
     omss[3]=db_SquareSum8Stride9(A+3);
     omss[4]=db_SquareSum8Stride9(A+4);
     omss[5]=db_SquareSum8Stride9(A+5);
     omss[6]=db_SquareSum8Stride9(A+6);
     omss[7]=db_SquareSum8Stride9(A+7);
     omss[8]=db_SquareSum8Stride9(A+8);
     i=db_MinIndex9(omss);
     /*orthogonalize that row against all previous rows
     and normalize it*/
     B=A+i;
     db_MultiplyScalarCopy9(x,A,-B[0]);
     db_RowOperation9(x,A+9 ,B[9]);
     db_RowOperation9(x,A+18,B[18]);
     db_RowOperation9(x,A+27,B[27]);
     db_RowOperation9(x,A+36,B[36]);
     db_RowOperation9(x,A+45,B[45]);
     db_RowOperation9(x,A+54,B[54]);
     db_RowOperation9(x,A+63,B[63]);
     x[i]+=1.0;
     db_MultiplyScalar9(x,db_SafeSqrtReciprocal(1.0-omss[i]));
 }
	/*
	* Copyright (C) 2011 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	/* $Id: db_utilities_linalg.cpp,v 1.3 2011/06/17 14:03:31 mbansal Exp $ */

	#include "db_utilities_linalg.h"
	#include "db_utilities.h"



	/*****************************************************************
	* Lean and mean begins here *
	*****************************************************************/

	/*Cholesky-factorize symmetric positive definite 6 x 6 matrix A. Upper
	part of A is used from the input. The Cholesky factor is output as
	subdiagonal part in A and diagonal in d, which is 6-dimensional*/
	void db_CholeskyDecomp6x6(double A[36],double d[6])
	{
	double s,temp;

	/[50 mult 35 add 6sqrt=85flops 6func]/
	/i=0/
	s=A[0];
	d[0]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[0]);
	A[6]=A[1]*temp;
	A[12]=A[2]*temp;
	A[18]=A[3]*temp;
	A[24]=A[4]*temp;
	A[30]=A[5]*temp;
	/i=1/
	s=A[7]-A[6]*A[6];
	d[1]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[1]);
	A[13]=(A[8]-A[6]A[12])temp;
	A[19]=(A[9]-A[6]A[18])temp;
	A[25]=(A[10]-A[6]A[24])temp;
	A[31]=(A[11]-A[6]A[30])temp;
	/i=2/
	s=A[14]-A[12]A[12]-A[13]A[13];
	d[2]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[2]);
	A[20]=(A[15]-A[12]A[18]-A[13]A[19])*temp;
	A[26]=(A[16]-A[12]A[24]-A[13]A[25])*temp;
	A[32]=(A[17]-A[12]A[30]-A[13]A[31])*temp;
	/i=3/
	s=A[21]-A[18]A[18]-A[19]A[19]-A[20]*A[20];
	d[3]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[3]);
	A[27]=(A[22]-A[18]A[24]-A[19]A[25]-A[20]A[26])temp;
	A[33]=(A[23]-A[18]A[30]-A[19]A[31]-A[20]A[32])temp;
	/i=4/
	s=A[28]-A[24]A[24]-A[25]A[25]-A[26]A[26]-A[27]A[27];
	d[4]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[4]);
	A[34]=(A[29]-A[24]A[30]-A[25]A[31]-A[26]A[32]-A[27]A[33])*temp;
	/i=5/
	s=A[35]-A[30]A[30]-A[31]A[31]-A[32]A[32]-A[33]A[33]-A[34]*A[34];
	d[5]=((s>0.0)?sqrt(s):1.0);
	}

	/*Cholesky-factorize symmetric positive definite n x n matrix A.Part
	above diagonal of A is used from the input, diagonal of A is assumed to
	be stored in d. The Cholesky factor is output as
	subdiagonal part in A and diagonal in d, which is n-dimensional*/
	void db_CholeskyDecompSeparateDiagonal(double *A,double d,int n)
	{
	int i,j,k;
	double s;
	double temp = 0.0;

	for(i=0;i<n;i++) for(j=i;j<n;j++)
	{
	if(i==j) s=d[i];
	else s=A[i][j];
	for(k=i-1;k>=0;k--) s-=A[i][k]*A[j][k];
	if(i==j)
	{
	d[i]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[i]);
	}
	else A[j][i]=s*temp;
	}
	}

	/Backsubstitute L%transpose(L)x=b for x given the Cholesky decomposition
	of an n x n matrix and the right hand side b. The vector b is unchanged*/
	void db_CholeskyBacksub(double x,const double const A,const double d,int n,const double *b)
	{
	int i,k;
	double s;

	for(i=0;i<n;i++)
	{
	for(s=b[i],k=i-1;k>=0;k--) s-=A[i][k]*x[k];
	x[i]=db_SafeDivision(s,d[i]);
	}
	for(i=n-1;i>=0;i--)
	{
	for(s=x[i],k=i+1;k<n;k++) s-=A[k][i]*x[k];
	x[i]=db_SafeDivision(s,d[i]);
	}
	}

	/*Cholesky-factorize symmetric positive definite 3 x 3 matrix A. Part
	above diagonal of A is used from the input, diagonal of A is assumed to
	be stored in d. The Cholesky factor is output as subdiagonal part in A
	and diagonal in d, which is 3-dimensional*/
	void db_CholeskyDecomp3x3SeparateDiagonal(double A[9],double d[3])
	{
	double s,temp;

	/i=0/
	s=d[0];
	d[0]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[0]);
	A[3]=A[1]*temp;
	A[6]=A[2]*temp;
	/i=1/
	s=d[1]-A[3]*A[3];
	d[1]=((s>0.0)?sqrt(s):1.0);
	temp=db_SafeReciprocal(d[1]);
	A[7]=(A[5]-A[3]A[6])temp;
	/i=2/
	s=d[2]-A[6]A[6]-A[7]A[7];
	d[2]=((s>0.0)?sqrt(s):1.0);
	}

	/Backsubstitute L%transpose(L)x=b for x given the Cholesky decomposition
	of a 3 x 3 matrix and the right hand side b. The vector b is unchanged*/
	void db_CholeskyBacksub3x3(double x[3],const double A[9],const double d[3],const double b[3])
	{
	/[42 mult 30 add=72flops]/
	x[0]=db_SafeDivision(b[0],d[0]);
	x[1]=db_SafeDivision((b[1]-A[3]*x[0]),d[1]);
	x[2]=db_SafeDivision((b[2]-A[6]x[0]-A[7]x[1]),d[2]);
	x[2]=db_SafeDivision(x[2],d[2]);
	x[1]=db_SafeDivision((x[1]-A[7]*x[2]),d[1]);
	x[0]=db_SafeDivision((x[0]-A[6]x[2]-A[3]x[1]),d[0]);
	}

	/Backsubstitute L%transpose(L)x=b for x given the Cholesky decomposition
	of a 6 x 6 matrix and the right hand side b. The vector b is unchanged*/
	void db_CholeskyBacksub6x6(double x[6],const double A[36],const double d[6],const double b[6])
	{
	/[42 mult 30 add=72flops]/
	x[0]=db_SafeDivision(b[0],d[0]);
	x[1]=db_SafeDivision((b[1]-A[6]*x[0]),d[1]);
	x[2]=db_SafeDivision((b[2]-A[12]x[0]-A[13]x[1]),d[2]);
	x[3]=db_SafeDivision((b[3]-A[18]x[0]-A[19]x[1]-A[20]*x[2]),d[3]);
	x[4]=db_SafeDivision((b[4]-A[24]x[0]-A[25]x[1]-A[26]x[2]-A[27]x[3]),d[4]);
	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]);
	x[5]=db_SafeDivision(x[5],d[5]);
	x[4]=db_SafeDivision((x[4]-A[34]*x[5]),d[4]);
	x[3]=db_SafeDivision((x[3]-A[33]x[5]-A[27]x[4]),d[3]);
	x[2]=db_SafeDivision((x[2]-A[32]x[5]-A[26]x[4]-A[20]*x[3]),d[2]);
	x[1]=db_SafeDivision((x[1]-A[31]x[5]-A[25]x[4]-A[19]x[3]-A[13]x[2]),d[1]);
	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]);
	}


	void db_Orthogonalize6x7(double A[42],int orthonormalize)
	{
	int i;
	double ss[6];

	/Compute square sums of rows/
	ss[0]=db_SquareSum7(A);
	ss[1]=db_SquareSum7(A+7);
	ss[2]=db_SquareSum7(A+14);
	ss[3]=db_SquareSum7(A+21);
	ss[4]=db_SquareSum7(A+28);
	ss[5]=db_SquareSum7(A+35);

	ss[1]-=db_OrthogonalizePair7(A+7 ,A,ss[0]);
	ss[2]-=db_OrthogonalizePair7(A+14,A,ss[0]);
	ss[3]-=db_OrthogonalizePair7(A+21,A,ss[0]);
	ss[4]-=db_OrthogonalizePair7(A+28,A,ss[0]);
	ss[5]-=db_OrthogonalizePair7(A+35,A,ss[0]);

	/Pivot on largest ss (could also be done on ss/(original_ss))/
	i=db_MaxIndex5(ss+1);
	db_OrthogonalizationSwap7(A+7,i,ss+1);

	ss[2]-=db_OrthogonalizePair7(A+14,A+7,ss[1]);
	ss[3]-=db_OrthogonalizePair7(A+21,A+7,ss[1]);
	ss[4]-=db_OrthogonalizePair7(A+28,A+7,ss[1]);
	ss[5]-=db_OrthogonalizePair7(A+35,A+7,ss[1]);

	i=db_MaxIndex4(ss+2);
	db_OrthogonalizationSwap7(A+14,i,ss+2);

	ss[3]-=db_OrthogonalizePair7(A+21,A+14,ss[2]);
	ss[4]-=db_OrthogonalizePair7(A+28,A+14,ss[2]);
	ss[5]-=db_OrthogonalizePair7(A+35,A+14,ss[2]);

	i=db_MaxIndex3(ss+3);
	db_OrthogonalizationSwap7(A+21,i,ss+3);

	ss[4]-=db_OrthogonalizePair7(A+28,A+21,ss[3]);
	ss[5]-=db_OrthogonalizePair7(A+35,A+21,ss[3]);

	i=db_MaxIndex2(ss+4);
	db_OrthogonalizationSwap7(A+28,i,ss+4);

	ss[5]-=db_OrthogonalizePair7(A+35,A+28,ss[4]);

	if(orthonormalize)
	{
	db_MultiplyScalar7(A ,db_SafeSqrtReciprocal(ss[0]));
	db_MultiplyScalar7(A+7 ,db_SafeSqrtReciprocal(ss[1]));
	db_MultiplyScalar7(A+14,db_SafeSqrtReciprocal(ss[2]));
	db_MultiplyScalar7(A+21,db_SafeSqrtReciprocal(ss[3]));
	db_MultiplyScalar7(A+28,db_SafeSqrtReciprocal(ss[4]));
	db_MultiplyScalar7(A+35,db_SafeSqrtReciprocal(ss[5]));
	}
	}

	void db_Orthogonalize8x9(double A[72],int orthonormalize)
	{
	int i;
	double ss[8];

	/Compute square sums of rows/
	ss[0]=db_SquareSum9(A);
	ss[1]=db_SquareSum9(A+9);
	ss[2]=db_SquareSum9(A+18);
	ss[3]=db_SquareSum9(A+27);
	ss[4]=db_SquareSum9(A+36);
	ss[5]=db_SquareSum9(A+45);
	ss[6]=db_SquareSum9(A+54);
	ss[7]=db_SquareSum9(A+63);

	ss[1]-=db_OrthogonalizePair9(A+9 ,A,ss[0]);
	ss[2]-=db_OrthogonalizePair9(A+18,A,ss[0]);
	ss[3]-=db_OrthogonalizePair9(A+27,A,ss[0]);
	ss[4]-=db_OrthogonalizePair9(A+36,A,ss[0]);
	ss[5]-=db_OrthogonalizePair9(A+45,A,ss[0]);
	ss[6]-=db_OrthogonalizePair9(A+54,A,ss[0]);
	ss[7]-=db_OrthogonalizePair9(A+63,A,ss[0]);

	/Pivot on largest ss (could also be done on ss/(original_ss))/
	i=db_MaxIndex7(ss+1);
	db_OrthogonalizationSwap9(A+9,i,ss+1);

	ss[2]-=db_OrthogonalizePair9(A+18,A+9,ss[1]);
	ss[3]-=db_OrthogonalizePair9(A+27,A+9,ss[1]);
	ss[4]-=db_OrthogonalizePair9(A+36,A+9,ss[1]);
	ss[5]-=db_OrthogonalizePair9(A+45,A+9,ss[1]);
	ss[6]-=db_OrthogonalizePair9(A+54,A+9,ss[1]);
	ss[7]-=db_OrthogonalizePair9(A+63,A+9,ss[1]);

	i=db_MaxIndex6(ss+2);
	db_OrthogonalizationSwap9(A+18,i,ss+2);

	ss[3]-=db_OrthogonalizePair9(A+27,A+18,ss[2]);
	ss[4]-=db_OrthogonalizePair9(A+36,A+18,ss[2]);
	ss[5]-=db_OrthogonalizePair9(A+45,A+18,ss[2]);
	ss[6]-=db_OrthogonalizePair9(A+54,A+18,ss[2]);
	ss[7]-=db_OrthogonalizePair9(A+63,A+18,ss[2]);

	i=db_MaxIndex5(ss+3);
	db_OrthogonalizationSwap9(A+27,i,ss+3);

	ss[4]-=db_OrthogonalizePair9(A+36,A+27,ss[3]);
	ss[5]-=db_OrthogonalizePair9(A+45,A+27,ss[3]);
	ss[6]-=db_OrthogonalizePair9(A+54,A+27,ss[3]);
	ss[7]-=db_OrthogonalizePair9(A+63,A+27,ss[3]);

	i=db_MaxIndex4(ss+4);
	db_OrthogonalizationSwap9(A+36,i,ss+4);

	ss[5]-=db_OrthogonalizePair9(A+45,A+36,ss[4]);
	ss[6]-=db_OrthogonalizePair9(A+54,A+36,ss[4]);
	ss[7]-=db_OrthogonalizePair9(A+63,A+36,ss[4]);

	i=db_MaxIndex3(ss+5);
	db_OrthogonalizationSwap9(A+45,i,ss+5);

	ss[6]-=db_OrthogonalizePair9(A+54,A+45,ss[5]);
	ss[7]-=db_OrthogonalizePair9(A+63,A+45,ss[5]);

	i=db_MaxIndex2(ss+6);
	db_OrthogonalizationSwap9(A+54,i,ss+6);

	ss[7]-=db_OrthogonalizePair9(A+63,A+54,ss[6]);

	if(orthonormalize)
	{
	db_MultiplyScalar9(A ,db_SafeSqrtReciprocal(ss[0]));
	db_MultiplyScalar9(A+9 ,db_SafeSqrtReciprocal(ss[1]));
	db_MultiplyScalar9(A+18,db_SafeSqrtReciprocal(ss[2]));
	db_MultiplyScalar9(A+27,db_SafeSqrtReciprocal(ss[3]));
	db_MultiplyScalar9(A+36,db_SafeSqrtReciprocal(ss[4]));
	db_MultiplyScalar9(A+45,db_SafeSqrtReciprocal(ss[5]));
	db_MultiplyScalar9(A+54,db_SafeSqrtReciprocal(ss[6]));
	db_MultiplyScalar9(A+63,db_SafeSqrtReciprocal(ss[7]));
	}
	}

	void db_NullVectorOrthonormal6x7(double x[7],const double A[42])
	{
	int i;
	double omss[7];
	const double *B;

	/*Pivot by choosing row of the identity matrix
	(the one corresponding to column of A with smallest square sum)*/
	omss[0]=db_SquareSum6Stride7(A);
	omss[1]=db_SquareSum6Stride7(A+1);
	omss[2]=db_SquareSum6Stride7(A+2);
	omss[3]=db_SquareSum6Stride7(A+3);
	omss[4]=db_SquareSum6Stride7(A+4);
	omss[5]=db_SquareSum6Stride7(A+5);
	omss[6]=db_SquareSum6Stride7(A+6);
	i=db_MinIndex7(omss);
	/*orthogonalize that row against all previous rows
	and normalize it*/
	B=A+i;
	db_MultiplyScalarCopy7(x,A,-B[0]);
	db_RowOperation7(x,A+7 ,B[7]);
	db_RowOperation7(x,A+14,B[14]);
	db_RowOperation7(x,A+21,B[21]);
	db_RowOperation7(x,A+28,B[28]);
	db_RowOperation7(x,A+35,B[35]);
	x[i]+=1.0;
	db_MultiplyScalar7(x,db_SafeSqrtReciprocal(1.0-omss[i]));
	}

	void db_NullVectorOrthonormal8x9(double x[9],const double A[72])
	{
	int i;
	double omss[9];
	const double *B;

	/*Pivot by choosing row of the identity matrix
	(the one corresponding to column of A with smallest square sum)*/
	omss[0]=db_SquareSum8Stride9(A);
	omss[1]=db_SquareSum8Stride9(A+1);
	omss[2]=db_SquareSum8Stride9(A+2);
	omss[3]=db_SquareSum8Stride9(A+3);
	omss[4]=db_SquareSum8Stride9(A+4);
	omss[5]=db_SquareSum8Stride9(A+5);
	omss[6]=db_SquareSum8Stride9(A+6);
	omss[7]=db_SquareSum8Stride9(A+7);
	omss[8]=db_SquareSum8Stride9(A+8);
	i=db_MinIndex9(omss);
	/*orthogonalize that row against all previous rows
	and normalize it*/
	B=A+i;
	db_MultiplyScalarCopy9(x,A,-B[0]);
	db_RowOperation9(x,A+9 ,B[9]);
	db_RowOperation9(x,A+18,B[18]);
	db_RowOperation9(x,A+27,B[27]);
	db_RowOperation9(x,A+36,B[36]);
	db_RowOperation9(x,A+45,B[45]);
	db_RowOperation9(x,A+54,B[54]);
	db_RowOperation9(x,A+63,B[63]);
	x[i]+=1.0;
	db_MultiplyScalar9(x,db_SafeSqrtReciprocal(1.0-omss[i]));
	}