Source/System/Image/Squish/OSGSquishMaths.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 <cfloat>
  34
  35 namespace osgsquish {
  36
  37 Sym3x3 ComputeWeightedCovariance( int n, Vec3 const* points, float const* weights )
  38 {
  39         // compute the centroid
  40         float total = 0.0f;
  41         Vec3 centroid( 0.0f );
  42         for( int i = 0; i < n; ++i )
  43         {
  44                 total += weights[i];
  45                 centroid += weights[i]*points[i];
  46         }
  47         centroid /= total;
  48
  49         // accumulate the covariance matrix
  50         Sym3x3 covariance( 0.0f );
  51         for( int i = 0; i < n; ++i )
  52         {
  53                 Vec3 a = points[i] - centroid;
  54                 Vec3 b = weights[i]*a;
  55
  56                 covariance[0] += a.X()*b.X();
  57                 covariance[1] += a.X()*b.Y();
  58                 covariance[2] += a.X()*b.Z();
  59                 covariance[3] += a.Y()*b.Y();
  60                 covariance[4] += a.Y()*b.Z();
  61                 covariance[5] += a.Z()*b.Z();
  62         }
  63
  64         // return it
  65         return covariance;
  66 }
  67
  68 static Vec3 GetMultiplicity1Evector( Sym3x3 const& matrix, float evalue )
  69 {
  70         // compute M
  71         Sym3x3 m;
  72         m[0] = matrix[0] - evalue;
  73         m[1] = matrix[1];
  74         m[2] = matrix[2];
  75         m[3] = matrix[3] - evalue;
  76         m[4] = matrix[4];
  77         m[5] = matrix[5] - evalue;
  78
  79         // compute U
  80         Sym3x3 u;
  81         u[0] = m[3]*m[5] - m[4]*m[4];
  82         u[1] = m[2]*m[4] - m[1]*m[5];
  83         u[2] = m[1]*m[4] - m[2]*m[3];
  84         u[3] = m[0]*m[5] - m[2]*m[2];
  85         u[4] = m[1]*m[2] - m[4]*m[0];
  86         u[5] = m[0]*m[3] - m[1]*m[1];
  87
  88         // find the largest component
  89         float mc = std::fabs( u[0] );
  90         int mi = 0;
  91         for( int i = 1; i < 6; ++i )
  92         {
  93                 float c = std::fabs( u[i] );
  94                 if( c > mc )
  95                 {
  96                         mc = c;
  97                         mi = i;
  98                 }
  99         }
 100
 101         // pick the column with this component
 102         switch( mi )
 103         {
 104         case 0:
 105                 return Vec3( u[0], u[1], u[2] );
 106
 107         case 1:
 108         case 3:
 109                 return Vec3( u[1], u[3], u[4] );
 110
 111         default:
 112                 return Vec3( u[2], u[4], u[5] );
 113         }
 114 }
 115
 116 static Vec3 GetMultiplicity2Evector( Sym3x3 const& matrix, float evalue )
 117 {
 118         // compute M
 119         Sym3x3 m;
 120         m[0] = matrix[0] - evalue;
 121         m[1] = matrix[1];
 122         m[2] = matrix[2];
 123         m[3] = matrix[3] - evalue;
 124         m[4] = matrix[4];
 125         m[5] = matrix[5] - evalue;
 126
 127         // find the largest component
 128         float mc = std::fabs( m[0] );
 129         int mi = 0;
 130         for( int i = 1; i < 6; ++i )
 131         {
 132                 float c = std::fabs( m[i] );
 133                 if( c > mc )
 134                 {
 135                         mc = c;
 136                         mi = i;
 137                 }
 138         }
 139
 140         // pick the first eigenvector based on this index
 141         switch( mi )
 142         {
 143         case 0:
 144         case 1:
 145                 return Vec3( -m[1], m[0], 0.0f );
 146
 147         case 2:
 148                 return Vec3( m[2], 0.0f, -m[0] );
 149
 150         case 3:
 151         case 4:
 152                 return Vec3( 0.0f, -m[4], m[3] );
 153
 154         default:
 155                 return Vec3( 0.0f, -m[5], m[4] );
 156         }
 157 }
 158
 159 Vec3 ComputePrincipleComponent( Sym3x3 const& matrix )
 160 {
 161         // compute the cubic coefficients
 162         float c0 = matrix[0]*matrix[3]*matrix[5]
 163                 + 2.0f*matrix[1]*matrix[2]*matrix[4]
 164                 - matrix[0]*matrix[4]*matrix[4]
 165                 - matrix[3]*matrix[2]*matrix[2]
 166                 - matrix[5]*matrix[1]*matrix[1];
 167         float c1 = matrix[0]*matrix[3] + matrix[0]*matrix[5] + matrix[3]*matrix[5]
 168                 - matrix[1]*matrix[1] - matrix[2]*matrix[2] - matrix[4]*matrix[4];
 169         float c2 = matrix[0] + matrix[3] + matrix[5];
 170
 171         // compute the quadratic coefficients
 172         float a = c1 - ( 1.0f/3.0f )*c2*c2;
 173         float b = ( -2.0f/27.0f )*c2*c2*c2 + ( 1.0f/3.0f )*c1*c2 - c0;
 174
 175         // compute the root count check
 176         float Q = 0.25f*b*b + ( 1.0f/27.0f )*a*a*a;
 177
 178         // test the multiplicity
 179         if( FLT_EPSILON < Q )
 180         {
 181                 // only one root, which implies we have a multiple of the identity
 182         return Vec3( 1.0f );
 183         }
 184         else if( Q < -FLT_EPSILON )
 185         {
 186                 // three distinct roots
 187                 float theta = std::atan2( std::sqrt( -Q ), -0.5f*b );
 188                 float rho = std::sqrt( 0.25f*b*b - Q );
 189
 190                 float rt = std::pow( rho, 1.0f/3.0f );
 191                 float ct = std::cos( theta/3.0f );
 192                 float st = std::sin( theta/3.0f );
 193
 194                 float l1 = ( 1.0f/3.0f )*c2 + 2.0f*rt*ct;
 195                 float l2 = ( 1.0f/3.0f )*c2 - rt*( ct + float(sqrt( 3.0f ))*st );
 196                 float l3 = ( 1.0f/3.0f )*c2 - rt*( ct - float(sqrt( 3.0f ))*st );
 197
 198                 // pick the larger
 199                 if( std::fabs( l2 ) > std::fabs( l1 ) )
 200                         l1 = l2;
 201                 if( std::fabs( l3 ) > std::fabs( l1 ) )
 202                         l1 = l3;
 203
 204                 // get the eigenvector
 205                 return GetMultiplicity1Evector( matrix, l1 );
 206         }
 207         else // if( -FLT_EPSILON <= Q && Q <= FLT_EPSILON )
 208         {
 209                 // two roots
 210                 float rt;
 211                 if( b < 0.0f )
 212                         rt = -std::pow( -0.5f*b, 1.0f/3.0f );
 213                 else
 214                         rt = std::pow( 0.5f*b, 1.0f/3.0f );
 215
 216                 float l1 = ( 1.0f/3.0f )*c2 + rt;               // repeated
 217                 float l2 = ( 1.0f/3.0f )*c2 - 2.0f*rt;
 218
 219                 // get the eigenvector
 220                 if( std::fabs( l1 ) > std::fabs( l2 ) )
 221                         return GetMultiplicity2Evector( matrix, l1 );
 222                 else
 223                         return GetMultiplicity1Evector( matrix, l2 );
 224         }
 225 }
 226
 227 } // namespace squish