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
ComputeWeightedCovariance(int n,Vec3 const * points,float const * weights)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
ComputePrincipleComponent(Sym3x3 const & matrix)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
260