1 /******************************************************************************* 2 * Copyright 2011 See AUTHORS file. 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 ******************************************************************************/ 16 17 package com.badlogic.gdx.math; 18 19 import java.io.Serializable; 20 21 import com.badlogic.gdx.utils.NumberUtils; 22 23 /** A simple quaternion class. 24 * @see <a href="http://en.wikipedia.org/wiki/Quaternion">http://en.wikipedia.org/wiki/Quaternion</a> 25 * @author badlogicgames@gmail.com 26 * @author vesuvio 27 * @author xoppa */ 28 public class Quaternion implements Serializable { 29 private static final long serialVersionUID = -7661875440774897168L; 30 private static Quaternion tmp1 = new Quaternion(0, 0, 0, 0); 31 private static Quaternion tmp2 = new Quaternion(0, 0, 0, 0); 32 33 public float x; 34 public float y; 35 public float z; 36 public float w; 37 38 /** Constructor, sets the four components of the quaternion. 39 * @param x The x-component 40 * @param y The y-component 41 * @param z The z-component 42 * @param w The w-component */ Quaternion(float x, float y, float z, float w)43 public Quaternion (float x, float y, float z, float w) { 44 this.set(x, y, z, w); 45 } 46 Quaternion()47 public Quaternion () { 48 idt(); 49 } 50 51 /** Constructor, sets the quaternion components from the given quaternion. 52 * 53 * @param quaternion The quaternion to copy. */ Quaternion(Quaternion quaternion)54 public Quaternion (Quaternion quaternion) { 55 this.set(quaternion); 56 } 57 58 /** Constructor, sets the quaternion from the given axis vector and the angle around that axis in degrees. 59 * 60 * @param axis The axis 61 * @param angle The angle in degrees. */ Quaternion(Vector3 axis, float angle)62 public Quaternion (Vector3 axis, float angle) { 63 this.set(axis, angle); 64 } 65 66 /** Sets the components of the quaternion 67 * @param x The x-component 68 * @param y The y-component 69 * @param z The z-component 70 * @param w The w-component 71 * @return This quaternion for chaining */ set(float x, float y, float z, float w)72 public Quaternion set (float x, float y, float z, float w) { 73 this.x = x; 74 this.y = y; 75 this.z = z; 76 this.w = w; 77 return this; 78 } 79 80 /** Sets the quaternion components from the given quaternion. 81 * @param quaternion The quaternion. 82 * @return This quaternion for chaining. */ set(Quaternion quaternion)83 public Quaternion set (Quaternion quaternion) { 84 return this.set(quaternion.x, quaternion.y, quaternion.z, quaternion.w); 85 } 86 87 /** Sets the quaternion components from the given axis and angle around that axis. 88 * 89 * @param axis The axis 90 * @param angle The angle in degrees 91 * @return This quaternion for chaining. */ set(Vector3 axis, float angle)92 public Quaternion set (Vector3 axis, float angle) { 93 return setFromAxis(axis.x, axis.y, axis.z, angle); 94 } 95 96 /** @return a copy of this quaternion */ cpy()97 public Quaternion cpy () { 98 return new Quaternion(this); 99 } 100 101 /** @return the euclidean length of the specified quaternion */ len(final float x, final float y, final float z, final float w)102 public final static float len (final float x, final float y, final float z, final float w) { 103 return (float)Math.sqrt(x * x + y * y + z * z + w * w); 104 } 105 106 /** @return the euclidean length of this quaternion */ len()107 public float len () { 108 return (float)Math.sqrt(x * x + y * y + z * z + w * w); 109 } 110 111 @Override toString()112 public String toString () { 113 return "[" + x + "|" + y + "|" + z + "|" + w + "]"; 114 } 115 116 /** Sets the quaternion to the given euler angles in degrees. 117 * @param yaw the rotation around the y axis in degrees 118 * @param pitch the rotation around the x axis in degrees 119 * @param roll the rotation around the z axis degrees 120 * @return this quaternion */ setEulerAngles(float yaw, float pitch, float roll)121 public Quaternion setEulerAngles (float yaw, float pitch, float roll) { 122 return setEulerAnglesRad(yaw * MathUtils.degreesToRadians, pitch * MathUtils.degreesToRadians, roll 123 * MathUtils.degreesToRadians); 124 } 125 126 /** Sets the quaternion to the given euler angles in radians. 127 * @param yaw the rotation around the y axis in radians 128 * @param pitch the rotation around the x axis in radians 129 * @param roll the rotation around the z axis in radians 130 * @return this quaternion */ setEulerAnglesRad(float yaw, float pitch, float roll)131 public Quaternion setEulerAnglesRad (float yaw, float pitch, float roll) { 132 final float hr = roll * 0.5f; 133 final float shr = (float)Math.sin(hr); 134 final float chr = (float)Math.cos(hr); 135 final float hp = pitch * 0.5f; 136 final float shp = (float)Math.sin(hp); 137 final float chp = (float)Math.cos(hp); 138 final float hy = yaw * 0.5f; 139 final float shy = (float)Math.sin(hy); 140 final float chy = (float)Math.cos(hy); 141 final float chy_shp = chy * shp; 142 final float shy_chp = shy * chp; 143 final float chy_chp = chy * chp; 144 final float shy_shp = shy * shp; 145 146 x = (chy_shp * chr) + (shy_chp * shr); // cos(yaw/2) * sin(pitch/2) * cos(roll/2) + sin(yaw/2) * cos(pitch/2) * sin(roll/2) 147 y = (shy_chp * chr) - (chy_shp * shr); // sin(yaw/2) * cos(pitch/2) * cos(roll/2) - cos(yaw/2) * sin(pitch/2) * sin(roll/2) 148 z = (chy_chp * shr) - (shy_shp * chr); // cos(yaw/2) * cos(pitch/2) * sin(roll/2) - sin(yaw/2) * sin(pitch/2) * cos(roll/2) 149 w = (chy_chp * chr) + (shy_shp * shr); // cos(yaw/2) * cos(pitch/2) * cos(roll/2) + sin(yaw/2) * sin(pitch/2) * sin(roll/2) 150 return this; 151 } 152 153 /** Get the pole of the gimbal lock, if any. 154 * @return positive (+1) for north pole, negative (-1) for south pole, zero (0) when no gimbal lock */ getGimbalPole()155 public int getGimbalPole() { 156 final float t = y*x+z*w; 157 return t > 0.499f ? 1 : (t < -0.499f ? -1 : 0); 158 } 159 160 /** Get the roll euler angle in radians, which is the rotation around the z axis. Requires that this quaternion is normalized. 161 * @return the rotation around the z axis in radians (between -PI and +PI) */ getRollRad()162 public float getRollRad() { 163 final int pole = getGimbalPole(); 164 return pole == 0 ? MathUtils.atan2(2f*(w*z + y*x), 1f - 2f * (x*x + z*z)) : (float)pole * 2f * MathUtils.atan2(y, w); 165 } 166 167 /** Get the roll euler angle in degrees, which is the rotation around the z axis. Requires that this quaternion is normalized. 168 * @return the rotation around the z axis in degrees (between -180 and +180) */ getRoll()169 public float getRoll() { 170 return getRollRad() * MathUtils.radiansToDegrees; 171 } 172 173 /** Get the pitch euler angle in radians, which is the rotation around the x axis. Requires that this quaternion is normalized. 174 * @return the rotation around the x axis in radians (between -(PI/2) and +(PI/2)) */ getPitchRad()175 public float getPitchRad() { 176 final int pole = getGimbalPole(); 177 return pole == 0 ? (float)Math.asin(MathUtils.clamp(2f*(w*x-z*y), -1f, 1f)) : (float)pole * MathUtils.PI * 0.5f; 178 } 179 180 /** Get the pitch euler angle in degrees, which is the rotation around the x axis. Requires that this quaternion is normalized. 181 * @return the rotation around the x axis in degrees (between -90 and +90) */ getPitch()182 public float getPitch() { 183 return getPitchRad() * MathUtils.radiansToDegrees; 184 } 185 186 /** Get the yaw euler angle in radians, which is the rotation around the y axis. Requires that this quaternion is normalized. 187 * @return the rotation around the y axis in radians (between -PI and +PI) */ getYawRad()188 public float getYawRad() { 189 return getGimbalPole() == 0 ? MathUtils.atan2(2f*(y*w + x*z), 1f - 2f*(y*y+x*x)) : 0f; 190 } 191 192 /** Get the yaw euler angle in degrees, which is the rotation around the y axis. Requires that this quaternion is normalized. 193 * @return the rotation around the y axis in degrees (between -180 and +180) */ getYaw()194 public float getYaw() { 195 return getYawRad() * MathUtils.radiansToDegrees; 196 } 197 len2(final float x, final float y, final float z, final float w)198 public final static float len2 (final float x, final float y, final float z, final float w) { 199 return x * x + y * y + z * z + w * w; 200 } 201 202 /** @return the length of this quaternion without square root */ len2()203 public float len2 () { 204 return x * x + y * y + z * z + w * w; 205 } 206 207 /** Normalizes this quaternion to unit length 208 * @return the quaternion for chaining */ nor()209 public Quaternion nor () { 210 float len = len2(); 211 if (len != 0.f && !MathUtils.isEqual(len, 1f)) { 212 len = (float)Math.sqrt(len); 213 w /= len; 214 x /= len; 215 y /= len; 216 z /= len; 217 } 218 return this; 219 } 220 221 /** Conjugate the quaternion. 222 * 223 * @return This quaternion for chaining */ conjugate()224 public Quaternion conjugate () { 225 x = -x; 226 y = -y; 227 z = -z; 228 return this; 229 } 230 231 // TODO : this would better fit into the vector3 class 232 /** Transforms the given vector using this quaternion 233 * 234 * @param v Vector to transform */ transform(Vector3 v)235 public Vector3 transform (Vector3 v) { 236 tmp2.set(this); 237 tmp2.conjugate(); 238 tmp2.mulLeft(tmp1.set(v.x, v.y, v.z, 0)).mulLeft(this); 239 240 v.x = tmp2.x; 241 v.y = tmp2.y; 242 v.z = tmp2.z; 243 return v; 244 } 245 246 /** Multiplies this quaternion with another one in the form of this = this * other 247 * 248 * @param other Quaternion to multiply with 249 * @return This quaternion for chaining */ mul(final Quaternion other)250 public Quaternion mul (final Quaternion other) { 251 final float newX = this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y; 252 final float newY = this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z; 253 final float newZ = this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x; 254 final float newW = this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z; 255 this.x = newX; 256 this.y = newY; 257 this.z = newZ; 258 this.w = newW; 259 return this; 260 } 261 262 /** Multiplies this quaternion with another one in the form of this = this * other 263 * 264 * @param x the x component of the other quaternion to multiply with 265 * @param y the y component of the other quaternion to multiply with 266 * @param z the z component of the other quaternion to multiply with 267 * @param w the w component of the other quaternion to multiply with 268 * @return This quaternion for chaining */ mul(final float x, final float y, final float z, final float w)269 public Quaternion mul (final float x, final float y, final float z, final float w) { 270 final float newX = this.w * x + this.x * w + this.y * z - this.z * y; 271 final float newY = this.w * y + this.y * w + this.z * x - this.x * z; 272 final float newZ = this.w * z + this.z * w + this.x * y - this.y * x; 273 final float newW = this.w * w - this.x * x - this.y * y - this.z * z; 274 this.x = newX; 275 this.y = newY; 276 this.z = newZ; 277 this.w = newW; 278 return this; 279 } 280 281 /** Multiplies this quaternion with another one in the form of this = other * this 282 * 283 * @param other Quaternion to multiply with 284 * @return This quaternion for chaining */ mulLeft(Quaternion other)285 public Quaternion mulLeft (Quaternion other) { 286 final float newX = other.w * this.x + other.x * this.w + other.y * this.z - other.z * y; 287 final float newY = other.w * this.y + other.y * this.w + other.z * this.x - other.x * z; 288 final float newZ = other.w * this.z + other.z * this.w + other.x * this.y - other.y * x; 289 final float newW = other.w * this.w - other.x * this.x - other.y * this.y - other.z * z; 290 this.x = newX; 291 this.y = newY; 292 this.z = newZ; 293 this.w = newW; 294 return this; 295 } 296 297 /** Multiplies this quaternion with another one in the form of this = other * this 298 * 299 * @param x the x component of the other quaternion to multiply with 300 * @param y the y component of the other quaternion to multiply with 301 * @param z the z component of the other quaternion to multiply with 302 * @param w the w component of the other quaternion to multiply with 303 * @return This quaternion for chaining */ mulLeft(final float x, final float y, final float z, final float w)304 public Quaternion mulLeft (final float x, final float y, final float z, final float w) { 305 final float newX = w * this.x + x * this.w + y * this.z - z * this.y; 306 final float newY = w * this.y + y * this.w + z * this.x - x * this.z; 307 final float newZ = w * this.z + z * this.w + x * this.y - y * this.x; 308 final float newW = w * this.w - x * this.x - y * this.y - z * this.z; 309 this.x = newX; 310 this.y = newY; 311 this.z = newZ; 312 this.w = newW; 313 return this; 314 } 315 316 /** Add the x,y,z,w components of the passed in quaternion to the ones of this quaternion */ add(Quaternion quaternion)317 public Quaternion add(Quaternion quaternion){ 318 this.x += quaternion.x; 319 this.y += quaternion.y; 320 this.z += quaternion.z; 321 this.w += quaternion.w; 322 return this; 323 } 324 325 /** Add the x,y,z,w components of the passed in quaternion to the ones of this quaternion */ add(float qx, float qy, float qz, float qw)326 public Quaternion add(float qx, float qy, float qz, float qw){ 327 this.x += qx; 328 this.y += qy; 329 this.z += qz; 330 this.w += qw; 331 return this; 332 } 333 334 // TODO : the matrix4 set(quaternion) doesnt set the last row+col of the matrix to 0,0,0,1 so... that's why there is this 335 // method 336 /** Fills a 4x4 matrix with the rotation matrix represented by this quaternion. 337 * 338 * @param matrix Matrix to fill */ toMatrix(final float[] matrix)339 public void toMatrix (final float[] matrix) { 340 final float xx = x * x; 341 final float xy = x * y; 342 final float xz = x * z; 343 final float xw = x * w; 344 final float yy = y * y; 345 final float yz = y * z; 346 final float yw = y * w; 347 final float zz = z * z; 348 final float zw = z * w; 349 // Set matrix from quaternion 350 matrix[Matrix4.M00] = 1 - 2 * (yy + zz); 351 matrix[Matrix4.M01] = 2 * (xy - zw); 352 matrix[Matrix4.M02] = 2 * (xz + yw); 353 matrix[Matrix4.M03] = 0; 354 matrix[Matrix4.M10] = 2 * (xy + zw); 355 matrix[Matrix4.M11] = 1 - 2 * (xx + zz); 356 matrix[Matrix4.M12] = 2 * (yz - xw); 357 matrix[Matrix4.M13] = 0; 358 matrix[Matrix4.M20] = 2 * (xz - yw); 359 matrix[Matrix4.M21] = 2 * (yz + xw); 360 matrix[Matrix4.M22] = 1 - 2 * (xx + yy); 361 matrix[Matrix4.M23] = 0; 362 matrix[Matrix4.M30] = 0; 363 matrix[Matrix4.M31] = 0; 364 matrix[Matrix4.M32] = 0; 365 matrix[Matrix4.M33] = 1; 366 } 367 368 /** Sets the quaternion to an identity Quaternion 369 * @return this quaternion for chaining */ idt()370 public Quaternion idt () { 371 return this.set(0, 0, 0, 1); 372 } 373 374 /** @return If this quaternion is an identity Quaternion */ isIdentity()375 public boolean isIdentity () { 376 return MathUtils.isZero(x) && MathUtils.isZero(y) && MathUtils.isZero(z) && MathUtils.isEqual(w, 1f); 377 } 378 379 /** @return If this quaternion is an identity Quaternion */ isIdentity(final float tolerance)380 public boolean isIdentity (final float tolerance) { 381 return MathUtils.isZero(x, tolerance) && MathUtils.isZero(y, tolerance) && MathUtils.isZero(z, tolerance) 382 && MathUtils.isEqual(w, 1f, tolerance); 383 } 384 385 // todo : the setFromAxis(v3,float) method should replace the set(v3,float) method 386 /** Sets the quaternion components from the given axis and angle around that axis. 387 * 388 * @param axis The axis 389 * @param degrees The angle in degrees 390 * @return This quaternion for chaining. */ setFromAxis(final Vector3 axis, final float degrees)391 public Quaternion setFromAxis (final Vector3 axis, final float degrees) { 392 return setFromAxis(axis.x, axis.y, axis.z, degrees); 393 } 394 395 /** Sets the quaternion components from the given axis and angle around that axis. 396 * 397 * @param axis The axis 398 * @param radians The angle in radians 399 * @return This quaternion for chaining. */ setFromAxisRad(final Vector3 axis, final float radians)400 public Quaternion setFromAxisRad (final Vector3 axis, final float radians) { 401 return setFromAxisRad(axis.x, axis.y, axis.z, radians); 402 } 403 404 /** Sets the quaternion components from the given axis and angle around that axis. 405 * @param x X direction of the axis 406 * @param y Y direction of the axis 407 * @param z Z direction of the axis 408 * @param degrees The angle in degrees 409 * @return This quaternion for chaining. */ setFromAxis(final float x, final float y, final float z, final float degrees)410 public Quaternion setFromAxis (final float x, final float y, final float z, final float degrees) { 411 return setFromAxisRad(x, y, z, degrees * MathUtils.degreesToRadians); 412 } 413 414 /** Sets the quaternion components from the given axis and angle around that axis. 415 * @param x X direction of the axis 416 * @param y Y direction of the axis 417 * @param z Z direction of the axis 418 * @param radians The angle in radians 419 * @return This quaternion for chaining. */ setFromAxisRad(final float x, final float y, final float z, final float radians)420 public Quaternion setFromAxisRad (final float x, final float y, final float z, final float radians) { 421 float d = Vector3.len(x, y, z); 422 if (d == 0f) return idt(); 423 d = 1f / d; 424 float l_ang = radians < 0 ? MathUtils.PI2 - (-radians % MathUtils.PI2) : radians % MathUtils.PI2; 425 float l_sin = (float)Math.sin(l_ang / 2); 426 float l_cos = (float)Math.cos(l_ang / 2); 427 return this.set(d * x * l_sin, d * y * l_sin, d * z * l_sin, l_cos).nor(); 428 } 429 430 /** Sets the Quaternion from the given matrix, optionally removing any scaling. */ 431 public Quaternion setFromMatrix (boolean normalizeAxes, Matrix4 matrix) { 432 return setFromAxes(normalizeAxes, matrix.val[Matrix4.M00], matrix.val[Matrix4.M01], matrix.val[Matrix4.M02], 433 matrix.val[Matrix4.M10], matrix.val[Matrix4.M11], matrix.val[Matrix4.M12], matrix.val[Matrix4.M20], 434 matrix.val[Matrix4.M21], matrix.val[Matrix4.M22]); 435 } 436 437 /** Sets the Quaternion from the given rotation matrix, which must not contain scaling. */ 438 public Quaternion setFromMatrix (Matrix4 matrix) { 439 return setFromMatrix(false, matrix); 440 } 441 442 /** Sets the Quaternion from the given matrix, optionally removing any scaling. */ 443 public Quaternion setFromMatrix (boolean normalizeAxes, Matrix3 matrix) { 444 return setFromAxes(normalizeAxes, matrix.val[Matrix3.M00], matrix.val[Matrix3.M01], matrix.val[Matrix3.M02], 445 matrix.val[Matrix3.M10], matrix.val[Matrix3.M11], matrix.val[Matrix3.M12], matrix.val[Matrix3.M20], 446 matrix.val[Matrix3.M21], matrix.val[Matrix3.M22]); 447 } 448 449 /** Sets the Quaternion from the given rotation matrix, which must not contain scaling. */ 450 public Quaternion setFromMatrix (Matrix3 matrix) { 451 return setFromMatrix(false, matrix); 452 } 453 454 /** <p> 455 * Sets the Quaternion from the given x-, y- and z-axis which have to be orthonormal. 456 * </p> 457 * 458 * <p> 459 * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ which in turn took it from Graphics Gem code at 460 * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z. 461 * </p> 462 * 463 * @param xx x-axis x-coordinate 464 * @param xy x-axis y-coordinate 465 * @param xz x-axis z-coordinate 466 * @param yx y-axis x-coordinate 467 * @param yy y-axis y-coordinate 468 * @param yz y-axis z-coordinate 469 * @param zx z-axis x-coordinate 470 * @param zy z-axis y-coordinate 471 * @param zz z-axis z-coordinate */ 472 public Quaternion setFromAxes (float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { 473 return setFromAxes(false, xx, xy, xz, yx, yy, yz, zx, zy, zz); 474 } 475 476 /** <p> 477 * Sets the Quaternion from the given x-, y- and z-axis. 478 * </p> 479 * 480 * <p> 481 * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ which in turn took it from Graphics Gem code at 482 * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z. 483 * </p> 484 * 485 * @param normalizeAxes whether to normalize the axes (necessary when they contain scaling) 486 * @param xx x-axis x-coordinate 487 * @param xy x-axis y-coordinate 488 * @param xz x-axis z-coordinate 489 * @param yx y-axis x-coordinate 490 * @param yy y-axis y-coordinate 491 * @param yz y-axis z-coordinate 492 * @param zx z-axis x-coordinate 493 * @param zy z-axis y-coordinate 494 * @param zz z-axis z-coordinate */ 495 public Quaternion setFromAxes (boolean normalizeAxes, float xx, float xy, float xz, float yx, float yy, float yz, float zx, 496 float zy, float zz) { 497 if (normalizeAxes) { 498 final float lx = 1f / Vector3.len(xx, xy, xz); 499 final float ly = 1f / Vector3.len(yx, yy, yz); 500 final float lz = 1f / Vector3.len(zx, zy, zz); 501 xx *= lx; 502 xy *= lx; 503 xz *= lx; 504 yx *= ly; 505 yy *= ly; 506 yz *= ly; 507 zx *= lz; 508 zy *= lz; 509 zz *= lz; 510 } 511 // the trace is the sum of the diagonal elements; see 512 // http://mathworld.wolfram.com/MatrixTrace.html 513 final float t = xx + yy + zz; 514 515 // we protect the division by s by ensuring that s>=1 516 if (t >= 0) { // |w| >= .5 517 float s = (float)Math.sqrt(t + 1); // |s|>=1 ... 518 w = 0.5f * s; 519 s = 0.5f / s; // so this division isn't bad 520 x = (zy - yz) * s; 521 y = (xz - zx) * s; 522 z = (yx - xy) * s; 523 } else if ((xx > yy) && (xx > zz)) { 524 float s = (float)Math.sqrt(1.0 + xx - yy - zz); // |s|>=1 525 x = s * 0.5f; // |x| >= .5 526 s = 0.5f / s; 527 y = (yx + xy) * s; 528 z = (xz + zx) * s; 529 w = (zy - yz) * s; 530 } else if (yy > zz) { 531 float s = (float)Math.sqrt(1.0 + yy - xx - zz); // |s|>=1 532 y = s * 0.5f; // |y| >= .5 533 s = 0.5f / s; 534 x = (yx + xy) * s; 535 z = (zy + yz) * s; 536 w = (xz - zx) * s; 537 } else { 538 float s = (float)Math.sqrt(1.0 + zz - xx - yy); // |s|>=1 539 z = s * 0.5f; // |z| >= .5 540 s = 0.5f / s; 541 x = (xz + zx) * s; 542 y = (zy + yz) * s; 543 w = (yx - xy) * s; 544 } 545 546 return this; 547 } 548 549 /** Set this quaternion to the rotation between two vectors. 550 * @param v1 The base vector, which should be normalized. 551 * @param v2 The target vector, which should be normalized. 552 * @return This quaternion for chaining */ setFromCross(final Vector3 v1, final Vector3 v2)553 public Quaternion setFromCross (final Vector3 v1, final Vector3 v2) { 554 final float dot = MathUtils.clamp(v1.dot(v2), -1f, 1f); 555 final float angle = (float)Math.acos(dot); 556 return setFromAxisRad(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x, angle); 557 } 558 559 /** Set this quaternion to the rotation between two vectors. 560 * @param x1 The base vectors x value, which should be normalized. 561 * @param y1 The base vectors y value, which should be normalized. 562 * @param z1 The base vectors z value, which should be normalized. 563 * @param x2 The target vector x value, which should be normalized. 564 * @param y2 The target vector y value, which should be normalized. 565 * @param z2 The target vector z value, which should be normalized. 566 * @return This quaternion for chaining */ setFromCross(final float x1, final float y1, final float z1, final float x2, final float y2, final float z2)567 public Quaternion setFromCross (final float x1, final float y1, final float z1, final float x2, final float y2, final float z2) { 568 final float dot = MathUtils.clamp(Vector3.dot(x1, y1, z1, x2, y2, z2), -1f, 1f); 569 final float angle = (float)Math.acos(dot); 570 return setFromAxisRad(y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2, angle); 571 } 572 573 /** Spherical linear interpolation between this quaternion and the other quaternion, based on the alpha value in the range 574 * [0,1]. Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ 575 * @param end the end quaternion 576 * @param alpha alpha in the range [0,1] 577 * @return this quaternion for chaining */ slerp(Quaternion end, float alpha)578 public Quaternion slerp (Quaternion end, float alpha) { 579 final float d = this.x * end.x + this.y * end.y + this.z * end.z + this.w * end.w; 580 float absDot = d < 0.f ? -d : d; 581 582 // Set the first and second scale for the interpolation 583 float scale0 = 1f - alpha; 584 float scale1 = alpha; 585 586 // Check if the angle between the 2 quaternions was big enough to 587 // warrant such calculations 588 if ((1 - absDot) > 0.1) {// Get the angle between the 2 quaternions, 589 // and then store the sin() of that angle 590 final float angle = (float)Math.acos(absDot); 591 final float invSinTheta = 1f / (float)Math.sin(angle); 592 593 // Calculate the scale for q1 and q2, according to the angle and 594 // it's sine value 595 scale0 = ((float)Math.sin((1f - alpha) * angle) * invSinTheta); 596 scale1 = ((float)Math.sin((alpha * angle)) * invSinTheta); 597 } 598 599 if (d < 0.f) scale1 = -scale1; 600 601 // Calculate the x, y, z and w values for the quaternion by using a 602 // special form of linear interpolation for quaternions. 603 x = (scale0 * x) + (scale1 * end.x); 604 y = (scale0 * y) + (scale1 * end.y); 605 z = (scale0 * z) + (scale1 * end.z); 606 w = (scale0 * w) + (scale1 * end.w); 607 608 // Return the interpolated quaternion 609 return this; 610 } 611 612 /** 613 * Spherical linearly interpolates multiple quaternions and stores the result in this Quaternion. 614 * Will not destroy the data previously inside the elements of q. 615 * result = (q_1^w_1)*(q_2^w_2)* ... *(q_n^w_n) where w_i=1/n. 616 * @param q List of quaternions 617 * @return This quaternion for chaining */ slerp(Quaternion[] q)618 public Quaternion slerp (Quaternion[] q) { 619 620 //Calculate exponents and multiply everything from left to right 621 final float w = 1.0f/q.length; 622 set(q[0]).exp(w); 623 for(int i=1;i<q.length;i++) 624 mul(tmp1.set(q[i]).exp(w)); 625 nor(); 626 return this; 627 } 628 629 /** 630 * Spherical linearly interpolates multiple quaternions by the given weights and stores the result in this Quaternion. 631 * Will not destroy the data previously inside the elements of q or w. 632 * result = (q_1^w_1)*(q_2^w_2)* ... *(q_n^w_n) where the sum of w_i is 1. 633 * Lists must be equal in length. 634 * @param q List of quaternions 635 * @param w List of weights 636 * @return This quaternion for chaining */ slerp(Quaternion[] q, float[] w)637 public Quaternion slerp (Quaternion[] q, float[] w) { 638 639 //Calculate exponents and multiply everything from left to right 640 set(q[0]).exp(w[0]); 641 for(int i=1;i<q.length;i++) 642 mul(tmp1.set(q[i]).exp(w[i])); 643 nor(); 644 return this; 645 } 646 647 /** 648 * Calculates (this quaternion)^alpha where alpha is a real number and stores the result in this quaternion. 649 * See http://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power 650 * @param alpha Exponent 651 * @return This quaternion for chaining */ exp(float alpha)652 public Quaternion exp (float alpha) { 653 654 //Calculate |q|^alpha 655 float norm = len(); 656 float normExp = (float)Math.pow(norm, alpha); 657 658 //Calculate theta 659 float theta = (float)Math.acos(w / norm); 660 661 //Calculate coefficient of basis elements 662 float coeff = 0; 663 if(Math.abs(theta) < 0.001) //If theta is small enough, use the limit of sin(alpha*theta) / sin(theta) instead of actual value 664 coeff = normExp*alpha / norm; 665 else 666 coeff = (float)(normExp*Math.sin(alpha*theta) / (norm*Math.sin(theta))); 667 668 //Write results 669 w = (float)(normExp*Math.cos(alpha*theta)); 670 x *= coeff; 671 y *= coeff; 672 z *= coeff; 673 674 //Fix any possible discrepancies 675 nor(); 676 677 return this; 678 } 679 680 @Override hashCode()681 public int hashCode () { 682 final int prime = 31; 683 int result = 1; 684 result = prime * result + NumberUtils.floatToRawIntBits(w); 685 result = prime * result + NumberUtils.floatToRawIntBits(x); 686 result = prime * result + NumberUtils.floatToRawIntBits(y); 687 result = prime * result + NumberUtils.floatToRawIntBits(z); 688 return result; 689 } 690 691 @Override equals(Object obj)692 public boolean equals (Object obj) { 693 if (this == obj) { 694 return true; 695 } 696 if (obj == null) { 697 return false; 698 } 699 if (!(obj instanceof Quaternion)) { 700 return false; 701 } 702 Quaternion other = (Quaternion)obj; 703 return (NumberUtils.floatToRawIntBits(w) == NumberUtils.floatToRawIntBits(other.w)) 704 && (NumberUtils.floatToRawIntBits(x) == NumberUtils.floatToRawIntBits(other.x)) 705 && (NumberUtils.floatToRawIntBits(y) == NumberUtils.floatToRawIntBits(other.y)) 706 && (NumberUtils.floatToRawIntBits(z) == NumberUtils.floatToRawIntBits(other.z)); 707 } 708 709 /** Get the dot product between the two quaternions (commutative). 710 * @param x1 the x component of the first quaternion 711 * @param y1 the y component of the first quaternion 712 * @param z1 the z component of the first quaternion 713 * @param w1 the w component of the first quaternion 714 * @param x2 the x component of the second quaternion 715 * @param y2 the y component of the second quaternion 716 * @param z2 the z component of the second quaternion 717 * @param w2 the w component of the second quaternion 718 * @return the dot product between the first and second quaternion. */ dot(final float x1, final float y1, final float z1, final float w1, final float x2, final float y2, final float z2, final float w2)719 public final static float dot (final float x1, final float y1, final float z1, final float w1, final float x2, final float y2, 720 final float z2, final float w2) { 721 return x1 * x2 + y1 * y2 + z1 * z2 + w1 * w2; 722 } 723 724 /** Get the dot product between this and the other quaternion (commutative). 725 * @param other the other quaternion. 726 * @return the dot product of this and the other quaternion. */ dot(final Quaternion other)727 public float dot (final Quaternion other) { 728 return this.x * other.x + this.y * other.y + this.z * other.z + this.w * other.w; 729 } 730 731 /** Get the dot product between this and the other quaternion (commutative). 732 * @param x the x component of the other quaternion 733 * @param y the y component of the other quaternion 734 * @param z the z component of the other quaternion 735 * @param w the w component of the other quaternion 736 * @return the dot product of this and the other quaternion. */ dot(final float x, final float y, final float z, final float w)737 public float dot (final float x, final float y, final float z, final float w) { 738 return this.x * x + this.y * y + this.z * z + this.w * w; 739 } 740 741 /** Multiplies the components of this quaternion with the given scalar. 742 * @param scalar the scalar. 743 * @return this quaternion for chaining. */ mul(float scalar)744 public Quaternion mul (float scalar) { 745 this.x *= scalar; 746 this.y *= scalar; 747 this.z *= scalar; 748 this.w *= scalar; 749 return this; 750 } 751 752 /** Get the axis angle representation of the rotation in degrees. The supplied vector will receive the axis (x, y and z values) 753 * of the rotation and the value returned is the angle in degrees around that axis. Note that this method will alter the 754 * supplied vector, the existing value of the vector is ignored. </p> This will normalize this quaternion if needed. The 755 * received axis is a unit vector. However, if this is an identity quaternion (no rotation), then the length of the axis may be 756 * zero. 757 * 758 * @param axis vector which will receive the axis 759 * @return the angle in degrees 760 * @see <a href="http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation">wikipedia</a> 761 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle">calculation</a> */ getAxisAngle(Vector3 axis)762 public float getAxisAngle (Vector3 axis) { 763 return getAxisAngleRad(axis) * MathUtils.radiansToDegrees; 764 } 765 766 /** Get the axis-angle representation of the rotation in radians. The supplied vector will receive the axis (x, y and z values) 767 * of the rotation and the value returned is the angle in radians around that axis. Note that this method will alter the 768 * supplied vector, the existing value of the vector is ignored. </p> This will normalize this quaternion if needed. The 769 * received axis is a unit vector. However, if this is an identity quaternion (no rotation), then the length of the axis may be 770 * zero. 771 * 772 * @param axis vector which will receive the axis 773 * @return the angle in radians 774 * @see <a href="http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation">wikipedia</a> 775 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle">calculation</a> */ getAxisAngleRad(Vector3 axis)776 public float getAxisAngleRad (Vector3 axis) { 777 if (this.w > 1) this.nor(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised 778 float angle = (float)(2.0 * Math.acos(this.w)); 779 double s = Math.sqrt(1 - this.w * this.w); // assuming quaternion normalised then w is less than 1, so term always positive. 780 if (s < MathUtils.FLOAT_ROUNDING_ERROR) { // test to avoid divide by zero, s is always positive due to sqrt 781 // if s close to zero then direction of axis not important 782 axis.x = this.x; // if it is important that axis is normalised then replace with x=1; y=z=0; 783 axis.y = this.y; 784 axis.z = this.z; 785 } else { 786 axis.x = (float)(this.x / s); // normalise axis 787 axis.y = (float)(this.y / s); 788 axis.z = (float)(this.z / s); 789 } 790 791 return angle; 792 } 793 794 /** Get the angle in radians of the rotation this quaternion represents. Does not normalize the quaternion. Use 795 * {@link #getAxisAngleRad(Vector3)} to get both the axis and the angle of this rotation. Use 796 * {@link #getAngleAroundRad(Vector3)} to get the angle around a specific axis. 797 * @return the angle in radians of the rotation */ getAngleRad()798 public float getAngleRad () { 799 return (float)(2.0 * Math.acos((this.w > 1) ? (this.w / len()) : this.w)); 800 } 801 802 /** Get the angle in degrees of the rotation this quaternion represents. Use {@link #getAxisAngle(Vector3)} to get both the axis 803 * and the angle of this rotation. Use {@link #getAngleAround(Vector3)} to get the angle around a specific axis. 804 * @return the angle in degrees of the rotation */ getAngle()805 public float getAngle () { 806 return getAngleRad() * MathUtils.radiansToDegrees; 807 } 808 809 /** Get the swing rotation and twist rotation for the specified axis. The twist rotation represents the rotation around the 810 * specified axis. The swing rotation represents the rotation of the specified axis itself, which is the rotation around an 811 * axis perpendicular to the specified axis. 812 * </p> 813 * The swing and twist rotation can be used to reconstruct the original quaternion: this = swing * twist 814 * 815 * @param axisX the X component of the normalized axis for which to get the swing and twist rotation 816 * @param axisY the Y component of the normalized axis for which to get the swing and twist rotation 817 * @param axisZ the Z component of the normalized axis for which to get the swing and twist rotation 818 * @param swing will receive the swing rotation: the rotation around an axis perpendicular to the specified axis 819 * @param twist will receive the twist rotation: the rotation around the specified axis 820 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition">calculation</a> */ getSwingTwist(final float axisX, final float axisY, final float axisZ, final Quaternion swing, final Quaternion twist)821 public void getSwingTwist (final float axisX, final float axisY, final float axisZ, final Quaternion swing, 822 final Quaternion twist) { 823 final float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ); 824 twist.set(axisX * d, axisY * d, axisZ * d, this.w).nor(); 825 swing.set(twist).conjugate().mulLeft(this); 826 } 827 828 /** Get the swing rotation and twist rotation for the specified axis. The twist rotation represents the rotation around the 829 * specified axis. The swing rotation represents the rotation of the specified axis itself, which is the rotation around an 830 * axis perpendicular to the specified axis. 831 * </p> 832 * The swing and twist rotation can be used to reconstruct the original quaternion: this = swing * twist 833 * 834 * @param axis the normalized axis for which to get the swing and twist rotation 835 * @param swing will receive the swing rotation: the rotation around an axis perpendicular to the specified axis 836 * @param twist will receive the twist rotation: the rotation around the specified axis 837 * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition">calculation</a> */ getSwingTwist(final Vector3 axis, final Quaternion swing, final Quaternion twist)838 public void getSwingTwist (final Vector3 axis, final Quaternion swing, final Quaternion twist) { 839 getSwingTwist(axis.x, axis.y, axis.z, swing, twist); 840 } 841 842 /** Get the angle in radians of the rotation around the specified axis. The axis must be normalized. 843 * @param axisX the x component of the normalized axis for which to get the angle 844 * @param axisY the y component of the normalized axis for which to get the angle 845 * @param axisZ the z component of the normalized axis for which to get the angle 846 * @return the angle in radians of the rotation around the specified axis */ getAngleAroundRad(final float axisX, final float axisY, final float axisZ)847 public float getAngleAroundRad (final float axisX, final float axisY, final float axisZ) { 848 final float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ); 849 final float l2 = Quaternion.len2(axisX * d, axisY * d, axisZ * d, this.w); 850 return MathUtils.isZero(l2) ? 0f : (float)(2.0 * Math.acos(MathUtils.clamp((float) (this.w / Math.sqrt(l2)), -1f, 1f))); 851 } 852 853 /** Get the angle in radians of the rotation around the specified axis. The axis must be normalized. 854 * @param axis the normalized axis for which to get the angle 855 * @return the angle in radians of the rotation around the specified axis */ getAngleAroundRad(final Vector3 axis)856 public float getAngleAroundRad (final Vector3 axis) { 857 return getAngleAroundRad(axis.x, axis.y, axis.z); 858 } 859 860 /** Get the angle in degrees of the rotation around the specified axis. The axis must be normalized. 861 * @param axisX the x component of the normalized axis for which to get the angle 862 * @param axisY the y component of the normalized axis for which to get the angle 863 * @param axisZ the z component of the normalized axis for which to get the angle 864 * @return the angle in degrees of the rotation around the specified axis */ getAngleAround(final float axisX, final float axisY, final float axisZ)865 public float getAngleAround (final float axisX, final float axisY, final float axisZ) { 866 return getAngleAroundRad(axisX, axisY, axisZ) * MathUtils.radiansToDegrees; 867 } 868 869 /** Get the angle in degrees of the rotation around the specified axis. The axis must be normalized. 870 * @param axis the normalized axis for which to get the angle 871 * @return the angle in degrees of the rotation around the specified axis */ getAngleAround(final Vector3 axis)872 public float getAngleAround (final Vector3 axis) { 873 return getAngleAround(axis.x, axis.y, axis.z); 874 } 875 } 876