1 /* 2 * Copyright (c) 2009-2010 jMonkeyEngine 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are 7 * met: 8 * 9 * * Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 12 * * Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 package com.jme3.math; 34 35 import java.util.logging.Level; 36 import java.util.logging.Logger; 37 38 public class Eigen3f implements java.io.Serializable { 39 40 static final long serialVersionUID = 1; 41 42 private static final Logger logger = Logger.getLogger(Eigen3f.class 43 .getName()); 44 45 float[] eigenValues = new float[3]; 46 Vector3f[] eigenVectors = new Vector3f[3]; 47 48 static final double ONE_THIRD_DOUBLE = 1.0 / 3.0; 49 static final double ROOT_THREE_DOUBLE = Math.sqrt(3.0); 50 51 52 public Eigen3f() { 53 54 } 55 56 public Eigen3f(Matrix3f data) { 57 calculateEigen(data); 58 } 59 60 public void calculateEigen(Matrix3f data) { 61 // prep work... 62 eigenVectors[0] = new Vector3f(); 63 eigenVectors[1] = new Vector3f(); 64 eigenVectors[2] = new Vector3f(); 65 66 Matrix3f scaledData = new Matrix3f(data); 67 float maxMagnitude = scaleMatrix(scaledData); 68 69 // Compute the eigenvalues using double-precision arithmetic. 70 double roots[] = new double[3]; 71 computeRoots(scaledData, roots); 72 eigenValues[0] = (float) roots[0]; 73 eigenValues[1] = (float) roots[1]; 74 eigenValues[2] = (float) roots[2]; 75 76 float[] maxValues = new float[3]; 77 Vector3f[] maxRows = new Vector3f[3]; 78 maxRows[0] = new Vector3f(); 79 maxRows[1] = new Vector3f(); 80 maxRows[2] = new Vector3f(); 81 82 for (int i = 0; i < 3; i++) { 83 Matrix3f tempMatrix = new Matrix3f(scaledData); 84 tempMatrix.m00 -= eigenValues[i]; 85 tempMatrix.m11 -= eigenValues[i]; 86 tempMatrix.m22 -= eigenValues[i]; 87 float[] val = new float[1]; 88 val[0] = maxValues[i]; 89 if (!positiveRank(tempMatrix, val, maxRows[i])) { 90 // Rank was 0 (or very close to 0), so just return. 91 // Rescale back to the original size. 92 if (maxMagnitude > 1f) { 93 for (int j = 0; j < 3; j++) { 94 eigenValues[j] *= maxMagnitude; 95 } 96 } 97 98 eigenVectors[0].set(Vector3f.UNIT_X); 99 eigenVectors[1].set(Vector3f.UNIT_Y); 100 eigenVectors[2].set(Vector3f.UNIT_Z); 101 return; 102 } 103 maxValues[i] = val[0]; 104 } 105 106 float maxCompare = maxValues[0]; 107 int i = 0; 108 if (maxValues[1] > maxCompare) { 109 maxCompare = maxValues[1]; 110 i = 1; 111 } 112 if (maxValues[2] > maxCompare) { 113 i = 2; 114 } 115 116 // use the largest row to compute and order the eigen vectors. 117 switch (i) { 118 case 0: 119 maxRows[0].normalizeLocal(); 120 computeVectors(scaledData, maxRows[0], 1, 2, 0); 121 break; 122 case 1: 123 maxRows[1].normalizeLocal(); 124 computeVectors(scaledData, maxRows[1], 2, 0, 1); 125 break; 126 case 2: 127 maxRows[2].normalizeLocal(); 128 computeVectors(scaledData, maxRows[2], 0, 1, 2); 129 break; 130 } 131 132 // Rescale the values back to the original size. 133 if (maxMagnitude > 1f) { 134 for (i = 0; i < 3; i++) { 135 eigenValues[i] *= maxMagnitude; 136 } 137 } 138 } 139 140 /** 141 * Scale the matrix so its entries are in [-1,1]. The scaling is applied 142 * only when at least one matrix entry has magnitude larger than 1. 143 * 144 * @return the max magnitude in this matrix 145 */ 146 private float scaleMatrix(Matrix3f mat) { 147 148 float max = FastMath.abs(mat.m00); 149 float abs = FastMath.abs(mat.m01); 150 151 if (abs > max) { 152 max = abs; 153 } 154 abs = FastMath.abs(mat.m02); 155 if (abs > max) { 156 max = abs; 157 } 158 abs = FastMath.abs(mat.m11); 159 if (abs > max) { 160 max = abs; 161 } 162 abs = FastMath.abs(mat.m12); 163 if (abs > max) { 164 max = abs; 165 } 166 abs = FastMath.abs(mat.m22); 167 if (abs > max) { 168 max = abs; 169 } 170 171 if (max > 1f) { 172 float fInvMax = 1f / max; 173 mat.multLocal(fInvMax); 174 } 175 176 return max; 177 } 178 179 /** 180 * Compute the eigenvectors of the given Matrix, using the 181 * @param mat 182 * @param vect 183 * @param index1 184 * @param index2 185 * @param index3 186 */ 187 private void computeVectors(Matrix3f mat, Vector3f vect, int index1, 188 int index2, int index3) { 189 Vector3f vectorU = new Vector3f(), vectorV = new Vector3f(); 190 Vector3f.generateComplementBasis(vectorU, vectorV, vect); 191 192 Vector3f tempVect = mat.mult(vectorU); 193 float p00 = eigenValues[index3] - vectorU.dot(tempVect); 194 float p01 = vectorV.dot(tempVect); 195 float p11 = eigenValues[index3] - vectorV.dot(mat.mult(vectorV)); 196 float invLength; 197 float max = FastMath.abs(p00); 198 int row = 0; 199 float fAbs = FastMath.abs(p01); 200 if (fAbs > max) { 201 max = fAbs; 202 } 203 fAbs = FastMath.abs(p11); 204 if (fAbs > max) { 205 max = fAbs; 206 row = 1; 207 } 208 209 if (max >= FastMath.ZERO_TOLERANCE) { 210 if (row == 0) { 211 invLength = FastMath.invSqrt(p00 * p00 + p01 * p01); 212 p00 *= invLength; 213 p01 *= invLength; 214 vectorU.mult(p01, eigenVectors[index3]) 215 .addLocal(vectorV.mult(p00)); 216 } else { 217 invLength = FastMath.invSqrt(p11 * p11 + p01 * p01); 218 p11 *= invLength; 219 p01 *= invLength; 220 vectorU.mult(p11, eigenVectors[index3]) 221 .addLocal(vectorV.mult(p01)); 222 } 223 } else { 224 if (row == 0) { 225 eigenVectors[index3] = vectorV; 226 } else { 227 eigenVectors[index3] = vectorU; 228 } 229 } 230 231 Vector3f vectorS = vect.cross(eigenVectors[index3]); 232 mat.mult(vect, tempVect); 233 p00 = eigenValues[index1] - vect.dot(tempVect); 234 p01 = vectorS.dot(tempVect); 235 p11 = eigenValues[index1] - vectorS.dot(mat.mult(vectorS)); 236 max = FastMath.abs(p00); 237 row = 0; 238 fAbs = FastMath.abs(p01); 239 if (fAbs > max) { 240 max = fAbs; 241 } 242 fAbs = FastMath.abs(p11); 243 if (fAbs > max) { 244 max = fAbs; 245 row = 1; 246 } 247 248 if (max >= FastMath.ZERO_TOLERANCE) { 249 if (row == 0) { 250 invLength = FastMath.invSqrt(p00 * p00 + p01 * p01); 251 p00 *= invLength; 252 p01 *= invLength; 253 eigenVectors[index1] = vect.mult(p01).add(vectorS.mult(p00)); 254 } else { 255 invLength = FastMath.invSqrt(p11 * p11 + p01 * p01); 256 p11 *= invLength; 257 p01 *= invLength; 258 eigenVectors[index1] = vect.mult(p11).add(vectorS.mult(p01)); 259 } 260 } else { 261 if (row == 0) { 262 eigenVectors[index1].set(vectorS); 263 } else { 264 eigenVectors[index1].set(vect); 265 } 266 } 267 268 eigenVectors[index3].cross(eigenVectors[index1], eigenVectors[index2]); 269 } 270 271 /** 272 * Check the rank of the given Matrix to determine if it is positive. While 273 * doing so, store the max magnitude entry in the given float store and the 274 * max row of the matrix in the Vector store. 275 * 276 * @param matrix 277 * the Matrix3f to analyze. 278 * @param maxMagnitudeStore 279 * a float array in which to store (in the 0th position) the max 280 * magnitude entry of the matrix. 281 * @param maxRowStore 282 * a Vector3f to store the values of the row of the matrix 283 * containing the max magnitude entry. 284 * @return true if the given matrix has a non 0 rank. 285 */ 286 private boolean positiveRank(Matrix3f matrix, float[] maxMagnitudeStore, Vector3f maxRowStore) { 287 // Locate the maximum-magnitude entry of the matrix. 288 maxMagnitudeStore[0] = -1f; 289 int iRow, iCol, iMaxRow = -1; 290 for (iRow = 0; iRow < 3; iRow++) { 291 for (iCol = iRow; iCol < 3; iCol++) { 292 float fAbs = FastMath.abs(matrix.get(iRow, iCol)); 293 if (fAbs > maxMagnitudeStore[0]) { 294 maxMagnitudeStore[0] = fAbs; 295 iMaxRow = iRow; 296 } 297 } 298 } 299 300 // Return the row containing the maximum, to be used for eigenvector 301 // construction. 302 maxRowStore.set(matrix.getRow(iMaxRow)); 303 304 return maxMagnitudeStore[0] >= FastMath.ZERO_TOLERANCE; 305 } 306 307 /** 308 * Generate the base eigen values of the given matrix using double precision 309 * math. 310 * 311 * @param mat 312 * the Matrix3f to analyze. 313 * @param rootsStore 314 * a double array to store the results in. Must be at least 315 * length 3. 316 */ 317 private void computeRoots(Matrix3f mat, double[] rootsStore) { 318 // Convert the unique matrix entries to double precision. 319 double a = mat.m00, b = mat.m01, c = mat.m02, 320 d = mat.m11, e = mat.m12, 321 f = mat.m22; 322 323 // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The 324 // eigenvalues are the roots to this equation, all guaranteed to be 325 // real-valued, because the matrix is symmetric. 326 double char0 = a * d * f + 2.0 * b * c * e - a 327 * e * e - d * c * c - f * b * b; 328 329 double char1 = a * d - b * b + a * f - c * c 330 + d * f - e * e; 331 332 double char2 = a + d + f; 333 334 // Construct the parameters used in classifying the roots of the 335 // equation and in solving the equation for the roots in closed form. 336 double char2Div3 = char2 * ONE_THIRD_DOUBLE; 337 double abcDiv3 = (char1 - char2 * char2Div3) * ONE_THIRD_DOUBLE; 338 if (abcDiv3 > 0.0) { 339 abcDiv3 = 0.0; 340 } 341 342 double mbDiv2 = 0.5 * (char0 + char2Div3 * (2.0 * char2Div3 * char2Div3 - char1)); 343 344 double q = mbDiv2 * mbDiv2 + abcDiv3 * abcDiv3 * abcDiv3; 345 if (q > 0.0) { 346 q = 0.0; 347 } 348 349 // Compute the eigenvalues by solving for the roots of the polynomial. 350 double magnitude = Math.sqrt(-abcDiv3); 351 double angle = Math.atan2(Math.sqrt(-q), mbDiv2) * ONE_THIRD_DOUBLE; 352 double cos = Math.cos(angle); 353 double sin = Math.sin(angle); 354 double root0 = char2Div3 + 2.0 * magnitude * cos; 355 double root1 = char2Div3 - magnitude 356 * (cos + ROOT_THREE_DOUBLE * sin); 357 double root2 = char2Div3 - magnitude 358 * (cos - ROOT_THREE_DOUBLE * sin); 359 360 // Sort in increasing order. 361 if (root1 >= root0) { 362 rootsStore[0] = root0; 363 rootsStore[1] = root1; 364 } else { 365 rootsStore[0] = root1; 366 rootsStore[1] = root0; 367 } 368 369 if (root2 >= rootsStore[1]) { 370 rootsStore[2] = root2; 371 } else { 372 rootsStore[2] = rootsStore[1]; 373 if (root2 >= rootsStore[0]) { 374 rootsStore[1] = root2; 375 } else { 376 rootsStore[1] = rootsStore[0]; 377 rootsStore[0] = root2; 378 } 379 } 380 } 381 382 public static void main(String[] args) { 383 Matrix3f mat = new Matrix3f(2, 1, 1, 1, 2, 1, 1, 1, 2); 384 Eigen3f eigenSystem = new Eigen3f(mat); 385 386 logger.info("eigenvalues = "); 387 for (int i = 0; i < 3; i++) 388 logger.log(Level.INFO, "{0} ", eigenSystem.getEigenValue(i)); 389 390 logger.info("eigenvectors = "); 391 for (int i = 0; i < 3; i++) { 392 Vector3f vector = eigenSystem.getEigenVector(i); 393 logger.info(vector.toString()); 394 mat.setColumn(i, vector); 395 } 396 logger.info(mat.toString()); 397 398 // eigenvalues = 399 // 1.000000 1.000000 4.000000 400 // eigenvectors = 401 // 0.411953 0.704955 0.577350 402 // 0.404533 -0.709239 0.577350 403 // -0.816485 0.004284 0.577350 404 } 405 406 public float getEigenValue(int i) { 407 return eigenValues[i]; 408 } 409 410 public Vector3f getEigenVector(int i) { 411 return eigenVectors[i]; 412 } 413 414 public float[] getEigenValues() { 415 return eigenValues; 416 } 417 418 public Vector3f[] getEigenVectors() { 419 return eigenVectors; 420 } 421 } 422
