1e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* 2e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Copyright (C) 2011 The Android Open Source Project 3e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 4e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Licensed under the Apache License, Version 2.0 (the "License"); 5e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * you may not use this file except in compliance with the License. 6e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * You may obtain a copy of the License at 7e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 8e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * http://www.apache.org/licenses/LICENSE-2.0 9e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 10e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Unless required by applicable law or agreed to in writing, software 11e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * distributed under the License is distributed on an "AS IS" BASIS, 12e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * See the License for the specific language governing permissions and 14e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * limitations under the License. 15e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 16e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 17e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* $Id: db_utilities_poly.h,v 1.2 2010/09/03 12:00:11 bsouthall Exp $ */ 18e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 19e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#ifndef DB_UTILITIES_POLY 20e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#define DB_UTILITIES_POLY 21e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 22e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include "db_utilities.h" 23e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 24e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 25e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 26e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/***************************************************************** 27e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen* Lean and mean begins here * 28e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*****************************************************************/ 29e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 30e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * \defgroup LMPolynomial (LM) Polynomial utilities (solvers, arithmetic, evaluation, etc.) 31e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 32e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*\{*/ 33e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 34e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 35e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenIn debug mode closed form quadratic solving takes on the order of 15 microseconds 36e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenwhile eig of the companion matrix takes about 1.1 milliseconds 37e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenSpeed-optimized code in release mode solves a quadratic in 0.3 microseconds on 450MHz 38e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 39e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_SolveQuadratic(double *roots,int *nr_roots,double a,double b,double c) 40e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 41e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double rs,srs,q; 42e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 43e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*For non-degenerate quadratics 44e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [5 mult 2 add 1 sqrt=7flops 1func]*/ 45e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(a==0.0) 46e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 47e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(b==0.0) *nr_roots=0; 48e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 49e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 50e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen roots[0]= -c/b; 51e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen *nr_roots=1; 52e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 53e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 54e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 55e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 56e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen rs=b*b-4.0*a*c; 57e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(rs>=0.0) 58e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 59e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen *nr_roots=2; 60e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen srs=sqrt(rs); 61e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen q= -0.5*(b+db_sign(b)*srs); 62e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen roots[0]=q/a; 63e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*If b is zero db_sign(b) returns 1, 64e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen so q is only zero when b=0 and c=0*/ 65e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(q==0.0) *nr_roots=1; 66e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else roots[1]=c/q; 67e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 68e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else *nr_roots=0; 69e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 70e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 71e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 72e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 73e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenIn debug mode closed form cubic solving takes on the order of 45 microseconds 74e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenwhile eig of the companion matrix takes about 1.3 milliseconds 75e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenSpeed-optimized code in release mode solves a cubic in 1.5 microseconds on 450MHz 76e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenFor a non-degenerate cubic with two roots, the first root is the single root and 77e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenthe second root is the double root 78e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen*/ 79e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenDB_API void db_SolveCubic(double *roots,int *nr_roots,double a,double b,double c,double d); 80e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 81e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenIn debug mode closed form quartic solving takes on the order of 0.1 milliseconds 82e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenwhile eig of the companion matrix takes about 1.5 milliseconds 83e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenSpeed-optimized code in release mode solves a quartic in 2.6 microseconds on 450MHz*/ 84e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenDB_API void db_SolveQuartic(double *roots,int *nr_roots,double a,double b,double c,double d,double e); 85e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 86e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenQuartic solving where a solution is forced when splitting into quadratics, which 87e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chencan be good if the quartic is sometimes in fact a quadratic, such as in absolute orientation 88e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenwhen the data is planar*/ 89e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenDB_API void db_SolveQuarticForced(double *roots,int *nr_roots,double a,double b,double c,double d,double e); 90e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 91e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline double db_PolyEval1(const double p[2],double x) 92e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 93e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return(p[0]+x*p[1]); 94e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 95e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 96e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyPoly1_1(double *d,const double *a,const double *b) 97e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 98e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0,a1; 99e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1; 100e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0];a1=a[1]; 101e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1]; 102e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 103e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]=a0*b0; 104e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]=a0*b1+a1*b0; 105e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]= a1*b1; 106e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 107e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 108e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyPoly0_2(double *d,const double *a,const double *b) 109e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 110e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 111e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2; 112e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 113e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2]; 114e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 115e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]=a0*b0; 116e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]=a0*b1; 117e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]=a0*b2; 118e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 119e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 120e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyPoly1_2(double *d,const double *a,const double *b) 121e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 122e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0,a1; 123e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2; 124e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0];a1=a[1]; 125e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2]; 126e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 127e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]=a0*b0; 128e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]=a0*b1+a1*b0; 129e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]=a0*b2+a1*b1; 130e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[3]= a1*b2; 131e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 132e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 133e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 134e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_MultiplyPoly1_3(double *d,const double *a,const double *b) 135e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 136e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0,a1; 137e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2,b3; 138e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0];a1=a[1]; 139e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2];b3=b[3]; 140e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 141e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]=a0*b0; 142e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]=a0*b1+a1*b0; 143e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]=a0*b2+a1*b1; 144e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[3]=a0*b3+a1*b2; 145e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[4]= a1*b3; 146e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 147e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 148e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenMultiply d=a*b where a is one degree and b is two degree*/ 149e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_AddPolyProduct0_1(double *d,const double *a,const double *b) 150e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 151e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 152e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1; 153e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 154e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1]; 155e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 156e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]+=a0*b0; 157e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]+=a0*b1; 158e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 159e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_AddPolyProduct0_2(double *d,const double *a,const double *b) 160e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 161e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 162e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2; 163e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 164e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2]; 165e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 166e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]+=a0*b0; 167e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]+=a0*b1; 168e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]+=a0*b2; 169e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 170e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 171e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenMultiply d=a*b where a is one degree and b is two degree*/ 172e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_SubtractPolyProduct0_0(double *d,const double *a,const double *b) 173e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 174e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 175e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0; 176e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 177e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0]; 178e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 179e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]-=a0*b0; 180e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 181e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 182e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_SubtractPolyProduct0_1(double *d,const double *a,const double *b) 183e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 184e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 185e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1; 186e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 187e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1]; 188e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 189e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]-=a0*b0; 190e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]-=a0*b1; 191e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 192e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 193e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_SubtractPolyProduct0_2(double *d,const double *a,const double *b) 194e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 195e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0; 196e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2; 197e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0]; 198e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2]; 199e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 200e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]-=a0*b0; 201e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]-=a0*b1; 202e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]-=a0*b2; 203e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 204e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 205e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_SubtractPolyProduct1_3(double *d,const double *a,const double *b) 206e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 207e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0,a1; 208e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double b0,b1,b2,b3; 209e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=a[0];a1=a[1]; 210e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen b0=b[0];b1=b[1];b2=b[2];b3=b[3]; 211e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 212e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[0]-=a0*b0; 213e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[1]-=a0*b1+a1*b0; 214e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[2]-=a0*b2+a1*b1; 215e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[3]-=a0*b3+a1*b2; 216e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d[4]-= a1*b3; 217e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 218e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 219e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_CharacteristicPolynomial4x4(double p[5],const double A[16]) 220e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 221e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*All two by two determinants of the first two rows*/ 222e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double two01[3],two02[3],two03[3],two12[3],two13[3],two23[3]; 223e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Polynomials representing third and fourth row of A*/ 224e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double P0[2],P1[2],P2[2],P3[2]; 225e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double P4[2],P5[2],P6[2],P7[2]; 226e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*All three by three determinants of the first three rows*/ 227e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double neg_three0[4],neg_three1[4],three2[4],three3[4]; 228e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 229e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute 2x2 determinants*/ 230e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two01[0]=A[0]*A[5]-A[1]*A[4]; 231e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two01[1]= -(A[0]+A[5]); 232e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two01[2]=1.0; 233e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 234e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two02[0]=A[0]*A[6]-A[2]*A[4]; 235e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two02[1]= -A[6]; 236e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 237e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two03[0]=A[0]*A[7]-A[3]*A[4]; 238e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two03[1]= -A[7]; 239e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 240e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two12[0]=A[1]*A[6]-A[2]*A[5]; 241e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two12[1]=A[2]; 242e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 243e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two13[0]=A[1]*A[7]-A[3]*A[5]; 244e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two13[1]=A[3]; 245e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 246e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen two23[0]=A[2]*A[7]-A[3]*A[6]; 247e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 248e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P0[0]=A[8]; 249e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P1[0]=A[9]; 250e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P2[0]=A[10];P2[1]= -1.0; 251e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P3[0]=A[11]; 252e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 253e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P4[0]=A[12]; 254e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P5[0]=A[13]; 255e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P6[0]=A[14]; 256e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen P7[0]=A[15];P7[1]= -1.0; 257e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 258e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute 3x3 determinants.Note that the highest 259e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen degree polynomial goes first and the smaller ones 260e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen are added or subtracted from it*/ 261e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyPoly1_1( neg_three0,P2,two13); 262e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_0(neg_three0,P1,two23); 263e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_1(neg_three0,P3,two12); 264e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 265e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyPoly1_1( neg_three1,P2,two03); 266e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_1(neg_three1,P3,two02); 267e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_0(neg_three1,P0,two23); 268e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 269e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyPoly0_2( three2,P3,two01); 270e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_AddPolyProduct0_1( three2,P0,two13); 271e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_1(three2,P1,two03); 272e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 273e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyPoly1_2( three3,P2,two01); 274e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_AddPolyProduct0_1( three3,P0,two12); 275e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_1(three3,P1,two02); 276e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 277e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute 4x4 determinants*/ 278e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyPoly1_3( p,P7,three3); 279e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_AddPolyProduct0_2( p,P4,neg_three0); 280e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_2(p,P5,neg_three1); 281e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_SubtractPolyProduct0_2(p,P6,three2); 282e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 283e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 284e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_RealEigenvalues4x4(double lambda[4],int *nr_roots,const double A[16],int forced=0) 285e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 286e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double p[5]; 287e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 288e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_CharacteristicPolynomial4x4(p,A); 289e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(forced) db_SolveQuarticForced(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); 290e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else db_SolveQuartic(lambda,nr_roots,p[4],p[3],p[2],p[1],p[0]); 291e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 292e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 293e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*! 294e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta ChenCompute the unit norm eigenvector v of the matrix A corresponding 295e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chento the eigenvalue lambda 296e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen[96mult 60add 1sqrt=156flops 1sqrt]*/ 297e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Cheninline void db_EigenVector4x4(double v[4],double lambda,const double A[16]) 298e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 299e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double a0,a5,a10,a15; 300e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double d01,d02,d03,d12,d13,d23; 301e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double e01,e02,e03,e12,e13,e23; 302e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double C[16],n0,n1,n2,n3,m; 303e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 304e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute diagonal 305e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [4add=4flops]*/ 306e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a0=A[0]-lambda; 307e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a5=A[5]-lambda; 308e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a10=A[10]-lambda; 309e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a15=A[15]-lambda; 310e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 311e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute 2x2 determinants of rows 1,2 and 3,4 312e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [24mult 12add=36flops]*/ 313e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d01=a0*a5 -A[1]*A[4]; 314e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d02=a0*A[6] -A[2]*A[4]; 315e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d03=a0*A[7] -A[3]*A[4]; 316e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d12=A[1]*A[6]-A[2]*a5; 317e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d13=A[1]*A[7]-A[3]*a5; 318e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen d23=A[2]*A[7]-A[3]*A[6]; 319e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 320e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e01=A[8]*A[13]-A[9] *A[12]; 321e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e02=A[8]*A[14]-a10 *A[12]; 322e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e03=A[8]*a15 -A[11]*A[12]; 323e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e12=A[9]*A[14]-a10 *A[13]; 324e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e13=A[9]*a15 -A[11]*A[13]; 325e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen e23=a10 *a15 -A[11]*A[14]; 326e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 327e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute matrix of cofactors 328e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [48mult 32 add=80flops*/ 329e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[0]= (a5 *e23-A[6]*e13+A[7]*e12); 330e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[1]= -(A[4]*e23-A[6]*e03+A[7]*e02); 331e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[2]= (A[4]*e13-a5 *e03+A[7]*e01); 332e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[3]= -(A[4]*e12-a5 *e02+A[6]*e01); 333e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 334e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[4]= -(A[1]*e23-A[2]*e13+A[3]*e12); 335e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[5]= (a0 *e23-A[2]*e03+A[3]*e02); 336e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[6]= -(a0 *e13-A[1]*e03+A[3]*e01); 337e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[7]= (a0 *e12-A[1]*e02+A[2]*e01); 338e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 339e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[8]= (A[13]*d23-A[14]*d13+a15 *d12); 340e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[9]= -(A[12]*d23-A[14]*d03+a15 *d02); 341e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[10]= (A[12]*d13-A[13]*d03+a15 *d01); 342e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[11]= -(A[12]*d12-A[13]*d02+A[14]*d01); 343e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 344e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[12]= -(A[9]*d23-a10 *d13+A[11]*d12); 345e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[13]= (A[8]*d23-a10 *d03+A[11]*d02); 346e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[14]= -(A[8]*d13-A[9]*d03+A[11]*d01); 347e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen C[15]= (A[8]*d12-A[9]*d02+a10 *d01); 348e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 349e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Compute square sums of rows 350e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [16mult 12add=28flops*/ 351e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen n0=db_sqr(C[0]) +db_sqr(C[1]) +db_sqr(C[2]) +db_sqr(C[3]); 352e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen n1=db_sqr(C[4]) +db_sqr(C[5]) +db_sqr(C[6]) +db_sqr(C[7]); 353e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen n2=db_sqr(C[8]) +db_sqr(C[9]) +db_sqr(C[10])+db_sqr(C[11]); 354e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen n3=db_sqr(C[12])+db_sqr(C[13])+db_sqr(C[14])+db_sqr(C[15]); 355e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 356e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen /*Take the largest norm row and normalize 357e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen [4mult 1 sqrt=4flops 1sqrt]*/ 358e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(n0>=n1 && n0>=n2 && n0>=n3) 359e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 360e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m=db_SafeReciprocal(sqrt(n0)); 361e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyScalarCopy4(v,C,m); 362e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 363e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else if(n1>=n2 && n1>=n3) 364e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 365e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m=db_SafeReciprocal(sqrt(n1)); 366e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyScalarCopy4(v,&(C[4]),m); 367e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 368e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else if(n2>=n3) 369e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 370e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m=db_SafeReciprocal(sqrt(n2)); 371e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyScalarCopy4(v,&(C[8]),m); 372e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 373e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen else 374e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 375e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m=db_SafeReciprocal(sqrt(n3)); 376e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen db_MultiplyScalarCopy4(v,&(C[12]),m); 377e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen } 378e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 379e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 380e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 381e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 382e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/*\}*/ 383e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#endif /* DB_UTILITIES_POLY */ 384