1 /** 2 * Computes the weighted average of multiple rotations represented as {@link Quaterniond} instances. 3 * <p> 4 * Instances of this class are <i>not</i> thread-safe. 5 * 6 * @author Kai Burjack 7 */ 8 module doml.quaternion_d_interpolator; 9 10 import Math = doml.math; 11 12 import doml.matrix_3d; 13 14 import doml.quaternion_d; 15 16 /* 17 * The MIT License 18 * 19 * Copyright (c) 2016-2021 JOML 20 %#%# Translated by jordan4ibanez 21 * 22 * Permission is hereby granted, free of charge, to any person obtaining a copy 23 * of this software and associated documentation files (the "Software"), to deal 24 * in the Software without restriction, including without limitation the rights 25 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 26 * copies of the Software, and to permit persons to whom the Software is 27 * furnished to do so, subject to the following conditions: 28 * 29 * The above copyright notice and this permission notice shall be included in 30 * all copies or substantial portions of the Software. 31 * 32 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 33 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 34 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 35 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 36 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 37 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 38 * THE SOFTWARE. 39 */ 40 41 /** 42 * Computes the weighted average of multiple rotations represented as {@link Quaterniond} instances. 43 * <p> 44 * Instances of this class are <i>not</i> thread-safe. 45 * 46 * @author Kai Burjack 47 */ 48 49 /** 50 * Performs singular value decomposition on {@link Matrix3d}. 51 * <p> 52 * This code was adapted from <a href="http://www.public.iastate.edu/~dicook/JSS/paper/code/svd.c">http://www.public.iastate.edu/</a>. 53 * 54 * @author Kai Burjack 55 */ 56 private static struct SvdDecomposition3d { 57 private double[3] rv1; 58 private double[3] w; 59 private double[9] v; 60 61 private double SIGN(double a, double b) { 62 return (b) >= 0.0 ? Math.abs(a) : -Math.abs(a); 63 } 64 65 void svd(double[] a, int maxIterations, Matrix3d destU, Matrix3d destV) { 66 int flag, i, its, j, jj, k, l = 0, nm = 0; 67 double c, f, h, s, x, y, z; 68 double anorm = 0.0, g = 0.0, scale = 0.0; 69 /* Householder reduction to bidiagonal form */ 70 for (i = 0; i < 3; i++) { 71 /* left-hand reduction */ 72 l = i + 1; 73 rv1[i] = scale * g; 74 g = s = scale = 0.0; 75 for (k = i; k < 3; k++) 76 scale += Math.abs(a[k + 3 * i]); 77 if (scale != 0.0) { 78 for (k = i; k < 3; k++) { 79 a[k + 3 * i] = (a[k + 3 * i] / scale); 80 s += (a[k + 3 * i] * a[k + 3 * i]); 81 } 82 f = a[i + 3 * i]; 83 g = -SIGN(Math.sqrt(s), f); 84 h = f * g - s; 85 a[i + 3 * i] = f - g; 86 if (i != 3 - 1) { 87 for (j = l; j < 3; j++) { 88 for (s = 0.0, k = i; k < 3; k++) 89 s += a[k + 3 * i] * a[k + 3 * j]; 90 f = s / h; 91 for (k = i; k < 3; k++) 92 a[k + 3 * j] += f * a[k + 3 * i]; 93 } 94 } 95 for (k = i; k < 3; k++) 96 a[k + 3 * i] = a[k + 3 * i] * scale; 97 } 98 w[i] = (scale * g); 99 100 /* right-hand reduction */ 101 g = s = scale = 0.0; 102 if (i < 3 && i != 3 - 1) { 103 for (k = l; k < 3; k++) 104 scale += Math.abs(a[i + 3 * k]); 105 if (scale != 0.0) { 106 for (k = l; k < 3; k++) { 107 a[i + 3 * k] = a[i + 3 * k] / scale; 108 s += a[i + 3 * k] * a[i + 3 * k]; 109 } 110 f = a[i + 3 * l]; 111 g = -SIGN(Math.sqrt(s), f); 112 h = f * g - s; 113 a[i + 3 * l] = f - g; 114 for (k = l; k < 3; k++) 115 rv1[k] = a[i + 3 * k] / h; 116 if (i != 3 - 1) { 117 for (j = l; j < 3; j++) { 118 for (s = 0.0, k = l; k < 3; k++) 119 s += a[j + 3 * k] * a[i + 3 * k]; 120 for (k = l; k < 3; k++) 121 a[j + 3 * k] += s * rv1[k]; 122 } 123 } 124 for (k = l; k < 3; k++) 125 a[i + 3 * k] = a[i + 3 * k] * scale; 126 } 127 } 128 anorm = Math.max(anorm, (Math.abs(w[i]) + Math.abs(rv1[i]))); 129 } 130 131 /* accumulate the right-hand transformation */ 132 for (i = 3 - 1; i >= 0; i--) { 133 if (i < 3 - 1) { 134 if (g != 0.0) { 135 for (j = l; j < 3; j++) 136 v[j + 3 * i] = (a[i + 3 * j] / a[i + 3 * l]) / g; 137 /* double division to avoid underflow */ 138 for (j = l; j < 3; j++) { 139 for (s = 0.0, k = l; k < 3; k++) 140 s += a[i + 3 * k] * v[k + 3 * j]; 141 for (k = l; k < 3; k++) 142 v[k + 3 * j] += s * v[k + 3 * i]; 143 } 144 } 145 for (j = l; j < 3; j++) 146 v[i + 3 * j] = v[j + 3 * i] = 0.0; 147 } 148 v[i + 3 * i] = 1.0; 149 g = rv1[i]; 150 l = i; 151 } 152 153 /* accumulate the left-hand transformation */ 154 for (i = 3 - 1; i >= 0; i--) { 155 l = i + 1; 156 g = w[i]; 157 if (i < 3 - 1) 158 for (j = l; j < 3; j++) 159 a[i + 3 * j] = 0.0; 160 if (g != 0.0) { 161 g = 1.0 / g; 162 if (i != 3 - 1) { 163 for (j = l; j < 3; j++) { 164 for (s = 0.0, k = l; k < 3; k++) 165 s += a[k + 3 * i] * a[k + 3 * j]; 166 f = s / a[i + 3 * i] * g; 167 for (k = i; k < 3; k++) 168 a[k + 3 * j] += f * a[k + 3 * i]; 169 } 170 } 171 for (j = i; j < 3; j++) 172 a[j + 3 * i] = a[j + 3 * i] * g; 173 } else { 174 for (j = i; j < 3; j++) 175 a[j + 3 * i] = 0.0; 176 } 177 ++a[i + 3 * i]; 178 } 179 180 /* diagonalize the bidiagonal form */ 181 for (k = 3 - 1; k >= 0; k--) { /* loop over singular values */ 182 for (its = 0; its < maxIterations; its++) { /* loop over allowed iterations */ 183 flag = 1; 184 for (l = k; l >= 0; l--) { /* test for splitting */ 185 nm = l - 1; 186 if (Math.abs(rv1[l]) + anorm == anorm) { 187 flag = 0; 188 break; 189 } 190 if (Math.abs(w[nm]) + anorm == anorm) 191 break; 192 } 193 if (flag != 0) { 194 c = 0.0; 195 s = 1.0; 196 for (i = l; i <= k; i++) { 197 f = s * rv1[i]; 198 if (Math.abs(f) + anorm != anorm) { 199 g = w[i]; 200 h = PYTHAG(f, g); 201 w[i] = h; 202 h = 1.0 / h; 203 c = g * h; 204 s = (-f * h); 205 for (j = 0; j < 3; j++) { 206 y = a[j + 3 * nm]; 207 z = a[j + 3 * i]; 208 a[j + 3 * nm] = y * c + z * s; 209 a[j + 3 * i] = z * c - y * s; 210 } 211 } 212 } 213 } 214 z = w[k]; 215 if (l == k) { /* convergence */ 216 if (z < 0.0) { /* make singular value nonnegative */ 217 w[k] = -z; 218 for (j = 0; j < 3; j++) 219 v[j + 3 * k] = (-v[j + 3 * k]); 220 } 221 break; 222 } 223 if (its == maxIterations - 1) { 224 return; // Do nothing 225 } 226 227 /* shift from bottom 2 x 2 minor */ 228 x = w[l]; 229 nm = k - 1; 230 y = w[nm]; 231 g = rv1[nm]; 232 h = rv1[k]; 233 f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); 234 g = PYTHAG(f, 1.0); 235 f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x; 236 237 /* next QR transformation */ 238 c = s = 1.0; 239 for (j = l; j <= nm; j++) { 240 i = j + 1; 241 g = rv1[i]; 242 y = w[i]; 243 h = s * g; 244 g = c * g; 245 z = PYTHAG(f, h); 246 rv1[j] = z; 247 c = f / z; 248 s = h / z; 249 f = x * c + g * s; 250 g = g * c - x * s; 251 h = y * s; 252 y = y * c; 253 for (jj = 0; jj < 3; jj++) { 254 x = v[jj + 3 * j]; 255 z = v[jj + 3 * i]; 256 v[jj + 3 * j] = x * c + z * s; 257 v[jj + 3 * i] = z * c - x * s; 258 } 259 z = PYTHAG(f, h); 260 w[j] = z; 261 if (z != 0.0) { 262 z = 1.0 / z; 263 c = f * z; 264 s = h * z; 265 } 266 f = (c * g) + (s * y); 267 x = (c * y) - (s * g); 268 for (jj = 0; jj < 3; jj++) { 269 y = a[jj + 3 * j]; 270 z = a[jj + 3 * i]; 271 a[jj + 3 * j] = y * c + z * s; 272 a[jj + 3 * i] = z * c - y * s; 273 } 274 } 275 rv1[l] = 0.0; 276 rv1[k] = f; 277 w[k] = x; 278 } 279 } 280 destU.set(a); 281 destV.set(v); 282 } 283 284 private static double PYTHAG(double a, double b) { 285 double at = Math.abs(a), bt = Math.abs(b), ct, result; 286 if (at > bt) { 287 ct = bt / at; 288 result = at * Math.sqrt(1.0 + ct * ct); 289 } else if (bt > 0.0) { 290 ct = at / bt; 291 result = bt * Math.sqrt(1.0 + ct * ct); 292 } else 293 result = 0.0; 294 return (result); 295 } 296 } 297 298 299 300 struct QuaterniondInterpolator { 301 302 private SvdDecomposition3d svdDecomposition3d = SvdDecomposition3d(); 303 private double[9] m; 304 private Matrix3d u = Matrix3d(); 305 private Matrix3d v = Matrix3d(); 306 307 /** 308 * Compute the weighted average of all of the quaternions given in <code>qs</code> using the specified interpolation factors <code>weights</code>, and store the result in <code>dest</code>. 309 * 310 * @param qs 311 * the quaternions to interpolate over 312 * @param weights 313 * the weights of each individual quaternion in <code>qs</code> 314 * @param maxSvdIterations 315 * the maximum number of iterations in the Singular Value Decomposition step used by this method 316 * @param dest 317 * will hold the result 318 * @return dest 319 */ 320 public Quaterniond computeWeightedAverage(Quaterniond[] qs, double[] weights, int maxSvdIterations, Quaterniond dest) { 321 double m00 = 0.0, m01 = 0.0, m02 = 0.0; 322 double m10 = 0.0, m11 = 0.0, m12 = 0.0; 323 double m20 = 0.0, m21 = 0.0, m22 = 0.0; 324 // Sum the rotation matrices of qs 325 for (int i = 0; i < qs.length; i++) { 326 Quaterniond q = qs[i]; 327 double dx = q.x + q.x; 328 double dy = q.y + q.y; 329 double dz = q.z + q.z; 330 double q00 = dx * q.x; 331 double q11 = dy * q.y; 332 double q22 = dz * q.z; 333 double q01 = dx * q.y; 334 double q02 = dx * q.z; 335 double q03 = dx * q.w; 336 double q12 = dy * q.z; 337 double q13 = dy * q.w; 338 double q23 = dz * q.w; 339 m00 += weights[i] * (1.0 - q11 - q22); 340 m01 += weights[i] * (q01 + q23); 341 m02 += weights[i] * (q02 - q13); 342 m10 += weights[i] * (q01 - q23); 343 m11 += weights[i] * (1.0 - q22 - q00); 344 m12 += weights[i] * (q12 + q03); 345 m20 += weights[i] * (q02 + q13); 346 m21 += weights[i] * (q12 - q03); 347 m22 += weights[i] * (1.0 - q11 - q00); 348 } 349 m[0] = m00; 350 m[1] = m01; 351 m[2] = m02; 352 m[3] = m10; 353 m[4] = m11; 354 m[5] = m12; 355 m[6] = m20; 356 m[7] = m21; 357 m[8] = m22; 358 // Compute the Singular Value Decomposition of 'm' 359 svdDecomposition3d.svd(m, maxSvdIterations, u, v); 360 // Compute rotation matrix 361 u.mul(v.transpose()); 362 // Build quaternion from it 363 return dest.setFromNormalized(u).normalize(); 364 } 365 366 }