maths.cc (6435B)
1 /* ----------------------------------------------------------------------------- 2 3 Copyright (c) 2006 Simon Brown si@sjbrown.co.uk 4 5 Permission is hereby granted, free of charge, to any person obtaining 6 a copy of this software and associated documentation files (the 7 "Software"), to deal in the Software without restriction, including 8 without limitation the rights to use, copy, modify, merge, publish, 9 distribute, sublicense, and/or sell copies of the Software, and to 10 permit persons to whom the Software is furnished to do so, subject to 11 the following conditions: 12 13 The above copyright notice and this permission notice shall be included 14 in all copies or substantial portions of the Software. 15 16 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 17 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 20 CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 21 TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 24 -------------------------------------------------------------------------- */ 25 26 /*! @file 27 28 The symmetric eigensystem solver algorithm is from 29 http://www.geometrictools.com/Documentation/EigenSymmetric3x3.pdf 30 */ 31 32 #include "maths.h" 33 #include "simd.h" 34 #include <cfloat> 35 36 namespace squish { 37 38 Sym3x3 ComputeWeightedCovariance( int n, Vec3 const* points, float const* weights ) 39 { 40 // compute the centroid 41 float total = 0.0f; 42 Vec3 centroid( 0.0f ); 43 for( int i = 0; i < n; ++i ) 44 { 45 total += weights[i]; 46 centroid += weights[i]*points[i]; 47 } 48 if( total > FLT_EPSILON ) 49 centroid /= total; 50 51 // accumulate the covariance matrix 52 Sym3x3 covariance( 0.0f ); 53 for( int i = 0; i < n; ++i ) 54 { 55 Vec3 a = points[i] - centroid; 56 Vec3 b = weights[i]*a; 57 58 covariance[0] += a.X()*b.X(); 59 covariance[1] += a.X()*b.Y(); 60 covariance[2] += a.X()*b.Z(); 61 covariance[3] += a.Y()*b.Y(); 62 covariance[4] += a.Y()*b.Z(); 63 covariance[5] += a.Z()*b.Z(); 64 } 65 66 // return it 67 return covariance; 68 } 69 70 #if 0 71 72 static Vec3 GetMultiplicity1Evector( Sym3x3 const& matrix, float evalue ) 73 { 74 // compute M 75 Sym3x3 m; 76 m[0] = matrix[0] - evalue; 77 m[1] = matrix[1]; 78 m[2] = matrix[2]; 79 m[3] = matrix[3] - evalue; 80 m[4] = matrix[4]; 81 m[5] = matrix[5] - evalue; 82 83 // compute U 84 Sym3x3 u; 85 u[0] = m[3]*m[5] - m[4]*m[4]; 86 u[1] = m[2]*m[4] - m[1]*m[5]; 87 u[2] = m[1]*m[4] - m[2]*m[3]; 88 u[3] = m[0]*m[5] - m[2]*m[2]; 89 u[4] = m[1]*m[2] - m[4]*m[0]; 90 u[5] = m[0]*m[3] - m[1]*m[1]; 91 92 // find the largest component 93 float mc = std::fabs( u[0] ); 94 int mi = 0; 95 for( int i = 1; i < 6; ++i ) 96 { 97 float c = std::fabs( u[i] ); 98 if( c > mc ) 99 { 100 mc = c; 101 mi = i; 102 } 103 } 104 105 // pick the column with this component 106 switch( mi ) 107 { 108 case 0: 109 return Vec3( u[0], u[1], u[2] ); 110 111 case 1: 112 case 3: 113 return Vec3( u[1], u[3], u[4] ); 114 115 default: 116 return Vec3( u[2], u[4], u[5] ); 117 } 118 } 119 120 static Vec3 GetMultiplicity2Evector( Sym3x3 const& matrix, float evalue ) 121 { 122 // compute M 123 Sym3x3 m; 124 m[0] = matrix[0] - evalue; 125 m[1] = matrix[1]; 126 m[2] = matrix[2]; 127 m[3] = matrix[3] - evalue; 128 m[4] = matrix[4]; 129 m[5] = matrix[5] - evalue; 130 131 // find the largest component 132 float mc = std::fabs( m[0] ); 133 int mi = 0; 134 for( int i = 1; i < 6; ++i ) 135 { 136 float c = std::fabs( m[i] ); 137 if( c > mc ) 138 { 139 mc = c; 140 mi = i; 141 } 142 } 143 144 // pick the first eigenvector based on this index 145 switch( mi ) 146 { 147 case 0: 148 case 1: 149 return Vec3( -m[1], m[0], 0.0f ); 150 151 case 2: 152 return Vec3( m[2], 0.0f, -m[0] ); 153 154 case 3: 155 case 4: 156 return Vec3( 0.0f, -m[4], m[3] ); 157 158 default: 159 return Vec3( 0.0f, -m[5], m[4] ); 160 } 161 } 162 163 Vec3 ComputePrincipleComponent( Sym3x3 const& matrix ) 164 { 165 // compute the cubic coefficients 166 float c0 = matrix[0]*matrix[3]*matrix[5] 167 + 2.0f*matrix[1]*matrix[2]*matrix[4] 168 - matrix[0]*matrix[4]*matrix[4] 169 - matrix[3]*matrix[2]*matrix[2] 170 - matrix[5]*matrix[1]*matrix[1]; 171 float c1 = matrix[0]*matrix[3] + matrix[0]*matrix[5] + matrix[3]*matrix[5] 172 - matrix[1]*matrix[1] - matrix[2]*matrix[2] - matrix[4]*matrix[4]; 173 float c2 = matrix[0] + matrix[3] + matrix[5]; 174 175 // compute the quadratic coefficients 176 float a = c1 - ( 1.0f/3.0f )*c2*c2; 177 float b = ( -2.0f/27.0f )*c2*c2*c2 + ( 1.0f/3.0f )*c1*c2 - c0; 178 179 // compute the root count check 180 float Q = 0.25f*b*b + ( 1.0f/27.0f )*a*a*a; 181 182 // test the multiplicity 183 if( FLT_EPSILON < Q ) 184 { 185 // only one root, which implies we have a multiple of the identity 186 return Vec3( 1.0f ); 187 } 188 else if( Q < -FLT_EPSILON ) 189 { 190 // three distinct roots 191 float theta = std::atan2( std::sqrt( -Q ), -0.5f*b ); 192 float rho = std::sqrt( 0.25f*b*b - Q ); 193 194 float rt = std::pow( rho, 1.0f/3.0f ); 195 float ct = std::cos( theta/3.0f ); 196 float st = std::sin( theta/3.0f ); 197 198 float l1 = ( 1.0f/3.0f )*c2 + 2.0f*rt*ct; 199 float l2 = ( 1.0f/3.0f )*c2 - rt*( ct + ( float )sqrt( 3.0f )*st ); 200 float l3 = ( 1.0f/3.0f )*c2 - rt*( ct - ( float )sqrt( 3.0f )*st ); 201 202 // pick the larger 203 if( std::fabs( l2 ) > std::fabs( l1 ) ) 204 l1 = l2; 205 if( std::fabs( l3 ) > std::fabs( l1 ) ) 206 l1 = l3; 207 208 // get the eigenvector 209 return GetMultiplicity1Evector( matrix, l1 ); 210 } 211 else // if( -FLT_EPSILON <= Q && Q <= FLT_EPSILON ) 212 { 213 // two roots 214 float rt; 215 if( b < 0.0f ) 216 rt = -std::pow( -0.5f*b, 1.0f/3.0f ); 217 else 218 rt = std::pow( 0.5f*b, 1.0f/3.0f ); 219 220 float l1 = ( 1.0f/3.0f )*c2 + rt; // repeated 221 float l2 = ( 1.0f/3.0f )*c2 - 2.0f*rt; 222 223 // get the eigenvector 224 if( std::fabs( l1 ) > std::fabs( l2 ) ) 225 return GetMultiplicity2Evector( matrix, l1 ); 226 else 227 return GetMultiplicity1Evector( matrix, l2 ); 228 } 229 } 230 231 #else 232 233 #define POWER_ITERATION_COUNT 8 234 235 Vec3 ComputePrincipleComponent( Sym3x3 const& matrix ) 236 { 237 Vec4 const row0( matrix[0], matrix[1], matrix[2], 0.0f ); 238 Vec4 const row1( matrix[1], matrix[3], matrix[4], 0.0f ); 239 Vec4 const row2( matrix[2], matrix[4], matrix[5], 0.0f ); 240 Vec4 v = VEC4_CONST( 1.0f ); 241 for( int i = 0; i < POWER_ITERATION_COUNT; ++i ) 242 { 243 // matrix multiply 244 Vec4 w = row0*v.SplatX(); 245 w = MultiplyAdd(row1, v.SplatY(), w); 246 w = MultiplyAdd(row2, v.SplatZ(), w); 247 248 // get max component from xyz in all channels 249 Vec4 a = Max(w.SplatX(), Max(w.SplatY(), w.SplatZ())); 250 251 // divide through and advance 252 v = w*Reciprocal(a); 253 } 254 return v.GetVec3(); 255 } 256 257 #endif 258 259 } // namespace squish