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