code.vuplus.com Git - vuplus_xbmc/blob - lib/libsquish/maths.cpp

   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