001/* 002* Quaternion -- A quaternion that represents the orientation between 003* two reference frames. 004* 005* Copyright (C) 2009-2025, by Joseph A. Huwaldt. All rights reserved. 006* 007* This library is free software; you can redistribute it and/or 008* modify it under the terms of the GNU Lesser General Public 009* License as published by the Free Software Foundation; either 010* version 2 of the License, or (at your option) any later version. 011* 012* This library is distributed in the hope that it will be useful, 013* but WITHOUT ANY WARRANTY; without even the implied warranty of 014* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 015* Lesser General Public License for more details. 016* 017* You should have received a copy of the GNU Lesser General Public License 018* along with this program; if not, write to the Free Software 019* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 020* Or visit: http://www.gnu.org/licenses/lgpl.html 021*/ 022package jahuwaldt.js.param; 023 024import jahuwaldt.tools.math.MathTools; 025import javax.measure.quantity.*; 026import javax.measure.unit.NonSI; 027import javax.measure.unit.SI; 028import javolution.context.ObjectFactory; 029import javolution.context.StackContext; 030import static javolution.lang.MathLib.abs; 031import static javolution.lang.MathLib.cos; 032import static javolution.lang.MathLib.sin; 033import javolution.text.Text; 034import javolution.text.TextBuilder; 035import javolution.xml.XMLFormat; 036import javolution.xml.XMLSerializable; 037import javolution.xml.stream.XMLStreamException; 038import org.jscience.mathematics.number.Float64; 039import org.jscience.mathematics.vector.DimensionException; 040import org.jscience.mathematics.vector.Float64Matrix; 041import org.jscience.mathematics.vector.Float64Vector; 042import org.jscience.mathematics.vector.Vector; 043 044 045/** 046* <p> This class represents a 4 element quaternion made up of Float64 047* elements. Quaternions may be used to represents a relative attitude 048* (attitude transformation) between two different reference 049* frames; B wrt A or BA. It can be used to transform coordinates in reference frame A 050* to reference frame B (A2B).</p> 051* 052* <p> The following quaternion definition is used: 053* <pre> 054* q.x = e.x * sin(phi/2); 055* q.y = e.y * sin(phi/2); 056* q.z = e.z * sin(phi/2); 057* q.w = cos(phi/2); 058* </pre> 059* 060* <p> Reference: Glaese, John, "Quaternions -- A Brief Exposition", Rev. 2, NASA MSFC, 1974 and<br> 061* Farrell, Jay A. and Matthew Barth, "The Global Positioning System & 062* Inertial Navigation", pp. 39 - 42.</p> 063* 064* <p> Modified by: Joseph A. Huwaldt </p> 065* 066* @author Joseph A. Huwaldt Date: January 28, 2009 067* @version February 22, 2025 068*/ 069public final class Quaternion extends AbstractRotation<Quaternion> implements XMLSerializable { 070 071 private static final long serialVersionUID = 203148779610744030L; 072 073 /** 074 * The index to the vector X component of this quaternion. 075 */ 076 public static final int X = 0; 077 078 /** 079 * The index to the vector Y component of this quaternion. 080 */ 081 public static final int Y = 1; 082 083 /** 084 * The index to the vector Z component of this quaternion. 085 */ 086 public static final int Z = 2; 087 088 /** 089 * The index to the scalar W component of this quaternion. 090 */ 091 public static final int W = 3; 092 093 094 /** 095 * The elements of the quaternion. 096 * Serialization is handled by this class since Float64Matrix is not Serializable. 097 */ 098 private transient Float64Vector _data; 099 100 101 /** 102 * Returns a {@link Quaternion} instance holding the specified <code>double</code> values 103 * for the quaternion elements. 104 * 105 * @param x the x value. 106 * @param y the y value. 107 * @param z the z value. 108 * @param w the w value. 109 * @return The quaternion having the specified values. 110 */ 111 public static Quaternion valueOf(double x, double y, double z, double w) { 112 Quaternion Q = Quaternion.newInstance(); 113 Q._data = Float64Vector.valueOf(x, y, z, w); 114 return Q; 115 } 116 117 /** 118 * Returns a {@link Quaternion} instance holding the specified <code>double</code> values 119 * for the quaternion elements in the order (x, y, z, w). 120 * 121 * @param values A 4 element array of quaternion elements (x, y, z, w). 122 * @return The quaternion having the specified values. 123 * @throws DimensionException if the input array does not have 4 elements. 124 */ 125 public static Quaternion valueOf(double[] values) { 126 if (values.length != 4) 127 throw new DimensionException( RESOURCES.getString("q4DVectorReqErr") ); 128 129 Quaternion Q = Quaternion.newInstance(); 130 Q._data = Float64Vector.valueOf(values); 131 132 return Q; 133 } 134 135 /** 136 * Returns a {@link Quaternion} instance containing the specified unit vector and scalar. 137 * 138 * @param vector The unit vector part of the quaternion. 139 * @param scalar The scalar part of the quaternion. 140 * @return The quaternion having the specified values. 141 */ 142 public static Quaternion valueOf(Vector3D<Dimensionless> vector, double scalar) { 143 144 Quaternion Q = Quaternion.newInstance(); 145 Q._data = Float64Vector.valueOf(vector.getValue(X), vector.getValue(Y), vector.getValue(Z), scalar); 146 147 return Q; 148 } 149 150 /** 151 * Returns a {@link Quaternion} instance containing the specified vector of Float64 values. 152 * The vector must have exactly 4 elements in the order (x, y, z, w). 153 * 154 * @param vector The vector of Float64 quaternion element values 155 * (must have dimension of 4). 156 * @return The quaternion having the specified values. 157 * @throws DimensionException if the input vector does not have 4 elements. 158 */ 159 public static Quaternion valueOf(Vector<Float64> vector) { 160 if (vector.getDimension() != 4) 161 throw new DimensionException( RESOURCES.getString("q4DVectorReqErr") ); 162 163 Quaternion Q = Quaternion.newInstance(); 164 Q._data = Float64Vector.valueOf(vector); 165 166 return Q; 167 } 168 169 /** 170 * Returns a {@link Quaternion} representing a rotation about an arbitrary axis. 171 * 172 * @param uv The unit vector axis of rotation expressed in a reference frame. 173 * @param phi The rotation angle about the specified rotation axis. 174 * @return The quaternion representing the specified rotation angle about 175 * the specified rotation axis. 176 */ 177 public static Quaternion valueOf(Vector3D<Dimensionless> uv, Parameter<Angle> phi) { 178 179 double phiR = phi.doubleValue(SI.RADIAN); 180 double cphi_2 = cos(phiR/2.0); 181 double sphi_2 = sin(phiR/2.0); 182 183 double x = uv.getValue(X)*sphi_2; 184 double y = uv.getValue(Y)*sphi_2; 185 double z = uv.getValue(Z)*sphi_2; 186 double w = cphi_2; 187 188 Quaternion Q = Quaternion.valueOf(x,y,z,w); 189 return Q; 190 } 191 192 /** 193 * Returns a new {@link Quaternion} instance constructed from the specified rotation transform. 194 * 195 * @param transform The Rotation transform to convert to a new quaternion. 196 * @return the quaternion representing the specified attitude transform. 197 */ 198 public static Quaternion valueOf(Rotation<?> transform) { 199 Quaternion q; 200 if (transform instanceof Quaternion) 201 q = copyOf((Quaternion)transform); 202 else 203 q = transform.toQuaternion(); 204 return q; 205 } 206 207 208 /** 209 * Returns the vector part of this quaternion. 210 * 211 * @return The vector part of this quaternion. 212 */ 213 public Vector3D<Dimensionless> getVector() { 214 Vector3D<Dimensionless> V = Vector3D.valueOf(_data.getValue(X), _data.getValue(Y), _data.getValue(Z), Dimensionless.UNIT); 215 return V; 216 } 217 218 /** 219 * Return the scalar part of this quaternion. 220 * 221 * @return The scalar part of this quaternion. 222 */ 223 public Float64 getScalar() { 224 return _data.get(W); 225 } 226 227 /** 228 * Return the scalar part of this quaternion as a floating point number. 229 * 230 * @return The scalar part of this quaternion as a floating point number. 231 */ 232 public double getScalarValue() { 233 return _data.getValue(W); 234 } 235 236 /** 237 * Returns the value of an element from this quaternion (0=x, 1=y, 2=z, 3=w). 238 * 239 * @param i the dimension index (0=x, 1=y, 2=z, 3=w). 240 * @return the value of the element at <code>i</code>. 241 * @throws IndexOutOfBoundsException <code>(i < 0) || (i >= dimension())</code> 242 */ 243 public Float64 get(int i) { 244 return _data.get(i); 245 } 246 247 /** 248 * Returns the value of a floating point number from this quaternion (0=x, 1=y, 2=z, 3=w). 249 * 250 * @param i the floating point number index (0=x, 1=y, 2=z, 3=w). 251 * @return the value of the floating point number at <code>i</code>. 252 * @throws IndexOutOfBoundsException <code>(i < 0) || (i >= dimension())</code> 253 */ 254 public double getValue(int i) { 255 return _data.getValue(i); 256 } 257 258 /** 259 * Returns the Euclidian norm, magnitude, or length of this quaternion (square root of the 260 * dot product of this quaternion and it's conjugate). 261 * 262 * @return <code>sqrt(this · this^*)</code>. 263 */ 264 public Float64 norm() { 265 return _data.norm(); 266 } 267 268 /** 269 * Returns the {@link #norm}, magnitude, or length value of this quaternion. 270 * 271 * @return <code>this.norm().doubleValue()</code>. 272 */ 273 public double normValue() { 274 return _data.normValue(); 275 } 276 277 /** 278 * Returns the negation of this quaternion (all elements are multiplied by -1). 279 * 280 * @return <code>-this</code>. 281 * @see #conjugate 282 */ 283 public Quaternion opposite() { 284 Quaternion Q = Quaternion.newInstance(); 285 Q._data = this._data.opposite(); 286 return Q; 287 } 288 289 /** 290 * Returns the conjugate or spatial inverse of this quaternion. 291 * 292 * @return <code>this^*</code>. 293 * @see <a href="http://en.wikipedia.org/wiki/Quaternion#Conjugation.2C_the_norm.2C_and_division"> 294 * Wikipedia: Quaternion Conjugation</a> 295 */ 296 public Quaternion conjugate() { 297 double x = -this.getValue(X); 298 double y = -this.getValue(Y); 299 double z = -this.getValue(Z); 300 double w = this.getValue(W); 301 302 Quaternion Q = Quaternion.valueOf(x,y,z,w); 303 return Q; 304 } 305 306 /** 307 * Returns the spatial inverse of this transformation: AB rather than BA. 308 * This implementation returns the conjugate of this quaternion. 309 * 310 * @return <code>this' = this^*</code> 311 */ 312 @Override 313 public Quaternion transpose() { 314 return this.conjugate(); 315 } 316 317 /** 318 * Returns the sum of this quaternion with the one specified. 319 * 320 * @param that the quaternion to be added. 321 * @return <code>this + that</code>. 322 */ 323 public Quaternion plus(Quaternion that) { 324 325 Quaternion Q = Quaternion.newInstance(); 326 Q._data = this._data.plus(that._data); 327 328 return Q; 329 } 330 331 /** 332 * Returns the difference between this quaternion and the one specified. 333 * 334 * @param that the quaternion to be subtracted. 335 * @return <code>this - that</code>. 336 */ 337 public Quaternion minus(Quaternion that) { 338 339 Quaternion Q = Quaternion.newInstance(); 340 Q._data = this._data.minus(that._data); 341 342 return Q; 343 } 344 345 /** 346 * Returns the product of this quaternion with the specified coefficient. 347 * 348 * @param k the coefficient multiplier. 349 * @return <code>this · k</code> 350 */ 351 public Quaternion times(Float64 k) { 352 Quaternion Q = Quaternion.newInstance(); 353 Q._data = this._data.times(k); 354 return Q; 355 } 356 357 /** 358 * Returns the product of this quaternion with the specified coefficient. 359 * 360 * @param k the coefficient multiplier. 361 * @return <code>this times k</code> 362 */ 363 public Quaternion times(double k) { 364 Quaternion Q = Quaternion.newInstance(); 365 Q._data = _data.times(k); 366 return Q; 367 } 368 369 /** 370 * Returns the quaternion product of this quaternion with the specified rotation transform. 371 * If this quaternion is BA and that is AC then the returned 372 * value is: BC = BA times AC (or C2B = A2B times C2A). 373 * 374 * @param that the rotation transform multiplier. 375 * @return <code>this times that</code> 376 */ 377 @Override 378 public Quaternion times(Rotation<?> that) { 379 StackContext.enter(); 380 try { 381 Quaternion thatQ = that.toQuaternion(); 382 Float64Matrix M = this.toLeftMatrix(); 383 Float64Vector q3 = M.times(thatQ.toFloat64Vector()); 384 385 Quaternion Q = Quaternion.valueOf(q3); 386 return StackContext.outerCopy(Q); 387 388 } finally { 389 StackContext.exit(); 390 } 391 } 392 393 /** 394 * Returns the quaternion division of this quaternion with the specified rotation transform. 395 * 396 * @param that the rotation transform divisor. 397 * @return <code>this / that = this times that^-1</code> 398 */ 399 public Quaternion divide(Rotation<?> that) { 400 Quaternion thatQ = that.toQuaternion(); 401 Quaternion Q = this.times(thatQ.inverse()); 402 return Q; 403 } 404 405 /** 406 * Returns the quaternion inverse of this quaternion. 407 * 408 * @return <code>this^-1 = this^* / norm(this)^2</code> 409 */ 410 public Quaternion inverse() { 411 double x = _data.getValue(X); 412 double y = _data.getValue(Y); 413 double z = _data.getValue(Z); 414 double w = _data.getValue(W); 415 double Nq = x*x + y*y + z*z + w*w; // = q^* x q 416 417 Quaternion Q = Quaternion.valueOf(-x/Nq,-y/Nq,-z/Nq,w/Nq); 418 return Q; 419 } 420 421 /** 422 * Transforms a 3D vector from frame A to B using this quaternion. 423 * 424 * @param v the vector expressed in frame A. 425 * @return the vector expressed in frame B. 426 */ 427 @Override 428 public <Q extends Quantity> Vector3D<Q> transform(Coordinate3D<Q> v) { 429 /* The following is slightly faster than: DCMatrix TM = this.toDCM(); V out = TM.times(v); 430 since it eliminates the overhead of creating a DCMatrix object. Otherwise it is identical. 431 */ 432 double x = _data.getValue(X); 433 double y = _data.getValue(Y); 434 double z = _data.getValue(Z); 435 double w = _data.getValue(W); 436 437 double Nq = x*x + y*y + z*z + w*w; // = q^* x q 438 if (Nq > 0.0) Nq = 2.0/Nq; 439 440 double Xs = x*Nq, Ys = y*Nq, Zs = z*Nq; 441 double wX = w*Xs, wY = w*Ys, wZ = w*Zs; 442 double xX = x*Xs, xY = x*Ys, xZ = x*Zs; 443 double yY = y*Ys, yZ = y*Zs, zZ = z*Zs; 444 445 Vector3D<Q> vec = v.toVector3D(); 446 double v1 = vec.getValue(X); 447 double v2 = vec.getValue(Y); 448 double v3 = vec.getValue(Z); 449 450 double v1new = (1.0 - (yY + zZ))*v1 + (xY - wZ)*v2 + (xZ + wY)*v3; 451 double v2new = (xY + wZ)*v1 + (1.0 - (xX + zZ))*v2 + (yZ - wX)*v3; 452 double v3new = (xZ - wY)*v1 + (yZ + wX)*v2 + (1.0 - (xX + yY))*v3; 453 454 Vector3D<Q> out = Vector3D.valueOf(v1new, v2new, v3new, v.getUnit()); 455 return out; 456 } 457 458 /** 459 * Returns this quaternion with unit length (a versor). 460 * 461 * @return <code>this.divide(norm(this))</code> 462 */ 463 public Quaternion toUnit() { 464 double magnitude = this.normValue(); 465 if (abs(magnitude - 1.0) <= Parameter.EPS) return this; 466 Quaternion Q = this.times(1.0/magnitude); 467 return Q; 468 } 469 470 /** 471 * Returns the values stored in this vector as a Float64Vector with 472 * the values ordered (x, y, z, w). The scalar is in the 4th element. 473 * 474 * @return The values stored in this vector as a Float64Vector with 475 * the values ordered (x, y, z, w). 476 */ 477 public Float64Vector toFloat64Vector() { 478 return _data; 479 } 480 481 /** 482 * Returns a direction cosine transformation matrix from this quaternion. 483 * 484 * @return a direction cosine matrix that converts from frame A to B. 485 * @see <a href="http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion"> 486 * Wikipedia: Rotation Matrix-Quaternion</a> 487 */ 488 @Override 489 public DCMatrix toDCM() { 490 // Algorithm From: "Quaternions", by Ken Shoemake 491 // http://www.cs.caltech.edu/courses/cs171/quatut.pdf 492 493 double x = _data.getValue(X); 494 double y = _data.getValue(Y); 495 double z = _data.getValue(Z); 496 double w = _data.getValue(W); 497 498 double Nq = x*x + y*y + z*z + w*w; // = q^* x q 499 if (Nq > 0.0) Nq = 2.0/Nq; 500 501 double Xs = x*Nq, Ys = y*Nq, Zs = z*Nq; 502 double wX = w*Xs, wY = w*Ys, wZ = w*Zs; 503 double xX = x*Xs, xY = x*Ys, xZ = x*Zs; 504 double yY = y*Ys, yZ = y*Zs, zZ = z*Zs; 505 506 Float64Vector row0 = Float64Vector.valueOf(1.0 - (yY + zZ), xY - wZ, xZ + wY); 507 Float64Vector row1 = Float64Vector.valueOf( xY + wZ, 1.0 - (xX + zZ), yZ - wX); 508 Float64Vector row2 = Float64Vector.valueOf( xZ - wY, yZ + wX, 1.0 - (xX + yY)); 509 DCMatrix M = DCMatrix.valueOf(row0, row1, row2); 510 511 return M; 512 } 513 514 /** 515 * Returns a quaternion representing this attitude transformation. 516 * 517 * @return a quaternion that converts from frame A to B. 518 * @see <a href="http://en.wikipedia.org/wiki/Quaternion">> 519 * Wikipedia: Quaternion</a> 520 */ 521 @Override 522 public Quaternion toQuaternion() { 523 return this; 524 } 525 526 /** 527 * <p> Compute the angular velocity quaternion matrix (Omega) from an angular velocity vector of three-space. 528 * Used to propagate quaternions: 529 * qdot = Omega * q ==> <code>Float64Vector qdot = Quaternion.omega(w).times(q.toFloat64Vector());</code></p> 530 * 531 * <p> Values are ordered as follows: 532 * <pre> 533 * M = 0.5* { 0 r -q p, = { [Omega]' | [w] } where [Omega] = skewsym([w]) 534 * -r 0 p q, {-----------------} and [w] = [p q r]' = ang. vel. vect. 535 * q -p 0 r, { -[w] | 0 } 536 * -p -q -r 0} 537 * </pre> 538 * 539 * 540 * @param w The angular velocity vector [p q r]'. 541 * @return A 4x4 angular velocity quaternion matrix in units of radians/second. 542 */ 543 public static Float64Matrix omega(Vector3D<AngularVelocity> w) { 544 545 w = w.to(AngularVelocity.UNIT); 546 double p = 0.5*w.getValue(X); 547 double q = 0.5*w.getValue(Y); 548 double r = 0.5*w.getValue(Z); 549 550 Float64Vector row0 = Float64Vector.valueOf( 0, r, -q, p ); 551 Float64Vector row1 = Float64Vector.valueOf( -r, 0, p, q ); 552 Float64Vector row2 = Float64Vector.valueOf( q, -p, 0, r ); 553 Float64Vector row3 = Float64Vector.valueOf( -p, -q, -r, 0 ); 554 555 Float64Matrix M = Float64Matrix.valueOf(row0, row1, row2, row3); 556 return M; 557 } 558 559 /** 560 * Returns a 4x4 matrix version of this quaternion used to multiply on the left (q*p = Lq*p) 561 * where "Lq" is the matrix returned by this method and "p" is treated as a 4-element column vector. 562 * Values are ordered as follows: 563 * <pre> 564 * M = { w -z y x, = { w*[E] + [Q] | [q] } where [Q] = skewsym([q]) and [q] = q.getVector() 565 * z w -x y, {----------------------} [E] = identity matrix 566 * -y x w z, { -[q] | w } 567 * -x -y -z w} 568 * </pre> 569 * 570 * @return 4x4 <code>Lq</code> matrix such that <code>Lq.times(p.toFloat64Vector()) == q.times(p).toFloat64Vector()</code> 571 */ 572 public Float64Matrix toLeftMatrix() { 573 double x = _data.getValue(X); 574 double y = _data.getValue(Y); 575 double z = _data.getValue(Z); 576 double w = _data.getValue(W); 577 578 Float64Vector row0 = Float64Vector.valueOf( w, -z, y, x); 579 Float64Vector row1 = Float64Vector.valueOf( z, w, -x, y); 580 Float64Vector row2 = Float64Vector.valueOf(-y, x, w, z); 581 Float64Vector row3 = Float64Vector.valueOf(-x, -y, -z, w); 582 583 Float64Matrix M = Float64Matrix.valueOf(row0, row1, row2, row3); 584 return M; 585 } 586 587 /** 588 * Returns a 4x4 matrix version of this quaternion used to multiply on the right (p*q = p*Rq) 589 * where "Rq" is the matrix returned by this method and "p" is treated as a 4-element column vector. 590 * Values are ordered as follows: 591 * <pre> 592 * M = { w z -y x, = { w*[E] + [Q]' | [q] } where [Q] = skewsym([q]) and [q] = q.getVector() 593 * -z w x y, {----------------------} [E] = identity matrix 594 * y -x w z, { -[q] | w } 595 * -x -y -z w} 596 * </pre> 597 * 598 * @return 4x4 <code>Rq</code> matrix such that 599 * <code>Rq.times(p.toFloat64Vector()) == p.times(q.toFloat64Vector()).toFloat64Vector()</code> 600 */ 601 public Float64Matrix toRightMatrix() { 602 double x = _data.getValue(X); 603 double y = _data.getValue(Y); 604 double z = _data.getValue(Z); 605 double w = _data.getValue(W); 606 607 Float64Vector row0 = Float64Vector.valueOf( w, z, -y, x); 608 Float64Vector row1 = Float64Vector.valueOf(-z, w, x, y); 609 Float64Vector row2 = Float64Vector.valueOf( y, -x, w, z); 610 Float64Vector row3 = Float64Vector.valueOf(-x, -y, -z, w); 611 612 Float64Matrix M = Float64Matrix.valueOf(row0, row1, row2, row3); 613 return M; 614 } 615 616 /** 617 * Returns a copy of this quaternion 618 * {@link javolution.context.AllocatorContext allocated} 619 * by the calling thread (possibly on the stack). 620 * 621 * @return an identical and independent copy of this quaternion. 622 */ 623 @Override 624 public Quaternion copy() { 625 return copyOf(this); 626 } 627 628 /** 629 * Returns the text representation of this quaternion. 630 * 631 * @return the text representation of this quaternion. 632 */ 633 @Override 634 public Text toText() { 635 final int dimension = 4; 636 TextBuilder tmp = TextBuilder.newInstance(); 637 if (this.isApproxEqual(IDENTITY)) 638 tmp.append("{IDENTITY}"); 639 else { 640 tmp.append('{'); 641 for (int i = 0; i < dimension; i++) { 642 tmp.append(get(i)); 643 if (i != dimension - 1) 644 tmp.append(", "); 645 } 646 tmp.append('}'); 647 } 648 Text txt = tmp.toText(); 649 TextBuilder.recycle(tmp); 650 return txt; 651 } 652 653 /** 654 * Compares this Rotation against the specified Rotation for approximate 655 * equality (a Rotation object with rotation is equal to this one to within the 656 * numerical roundoff tolerance). 657 * 658 * @param obj the Rotation object to compare with. 659 * @return <code>true</code> if this Rotation is approximately identical to that 660 * Rotation; <code>false</code> otherwise. 661 */ 662 @Override 663 public boolean isApproxEqual(Rotation<?> obj) { 664 if (this == obj) 665 return true; 666 if (obj == null) 667 return false; 668 669 // Check for approximate equality of components. 670 Quaternion thatQ = obj.toQuaternion(); 671 for (int i=0; i < 4; ++i) { 672 double thisVal = this.getValue(i); 673 double thatVal = thatQ.getValue(i); 674 if (!MathTools.isApproxEqual(thisVal, thatVal, Parameter.EPS10)) 675 return false; 676 } 677 678 return true; 679 } 680 681 /** 682 * Compares this Quaternion against the specified object for strict 683 * equality (same rotation type and same values). 684 * 685 * @param obj the object to compare with. 686 * @return <code>true</code> if this rotation is identical to that 687 * rotation; <code>false</code> otherwise. 688 */ 689 @Override 690 public boolean equals(Object obj) { 691 if (this == obj) 692 return true; 693 if ((obj == null) || (obj.getClass() != this.getClass())) 694 return false; 695 696 Quaternion that = (Quaternion)obj; 697 return this._data.equals(that._data); 698 } 699 700 /** 701 * Returns the hash code for this rotation. 702 * 703 * @return the hash code value. 704 */ 705 @Override 706 public int hashCode() { 707 int hash = 7; 708 709 int var_code = _data.hashCode(); 710 hash = hash*31 + var_code; 711 712 return hash; 713 } 714 715 716 /** 717 * During serialization, this will write out the Float64Vector as a 718 * simple series of <code>double</code> values. This method is ONLY called by the Java 719 * Serialization mechanism and should not otherwise be used. 720 * 721 * @param out The output stream to serialized this object to. 722 * @throws java.io.IOException if the output stream could not be written to. 723 */ 724 private void writeObject( java.io.ObjectOutputStream out ) throws java.io.IOException { 725 726 // Call the default write object method. 727 out.defaultWriteObject(); 728 729 // Write out the three coordinate values. 730 out.writeDouble( _data.getValue(X) ); 731 out.writeDouble( _data.getValue(Y) ); 732 out.writeDouble( _data.getValue(Z) ); 733 out.writeDouble( _data.getValue(W) ); 734 735 } 736 737 /** 738 * During de-serialization, this will handle the reconstruction 739 * of the Float64Vector. This method is ONLY called by the Java 740 * Serialization mechanism and should not otherwise be used. 741 * 742 * @param in The input stream to be de-serialized 743 * @throws java.io.IOException if there is a problem reading from the input stream. 744 * @throws ClassNotFoundException if the class could not be constructed. 745 */ 746 private void readObject( java.io.ObjectInputStream in ) throws java.io.IOException, ClassNotFoundException { 747 748 // Call the default read object method. 749 in.defaultReadObject(); 750 751 // Read in the three coordinate values. 752 double x = in.readDouble(); 753 double y = in.readDouble(); 754 double z = in.readDouble(); 755 double w = in.readDouble(); 756 757 _data = Float64Vector.valueOf(x,y,z,w); 758 759 } 760 761 /** 762 * Holds the default XML representation. For example: 763 * <pre> 764 * <Quaternion> 765 * <X value="1.0" /> 766 * <Y value="0.0" /> 767 * <Z value="2.0" /> 768 * <W value="1.0" /> 769 * </Quaternion> 770 * </pre> 771 */ 772 protected static final XMLFormat<Quaternion> XML = new XMLFormat<Quaternion>(Quaternion.class) { 773 774 @Override 775 public Quaternion newInstance(Class<Quaternion> cls, InputElement xml) throws XMLStreamException { 776 return FACTORY.object(); 777 } 778 779 @Override 780 public void read(InputElement xml, Quaternion Q) throws XMLStreamException { 781 782 double x = xml.get("X", Float64.class).doubleValue(); 783 double y = xml.get("Y", Float64.class).doubleValue(); 784 double z = xml.get("Z", Float64.class).doubleValue(); 785 double w = xml.get("W", Float64.class).doubleValue(); 786 Q._data = Float64Vector.valueOf(x,y,z,w); 787 788 if (xml.hasNext()) 789 throw new XMLStreamException("Too many elements"); 790 } 791 792 @Override 793 public void write(Quaternion Q, OutputElement xml) throws XMLStreamException { 794 795 xml.add(Q._data.get(X), "X", Float64.class); 796 xml.add(Q._data.get(Y), "Y", Float64.class); 797 xml.add(Q._data.get(Z), "Z", Float64.class); 798 xml.add(Q._data.get(W), "W", Float64.class); 799 800 } 801 }; 802 803 804 /////////////////////// 805 // Factory creation. // 806 /////////////////////// 807 808 private static final ObjectFactory<Quaternion> FACTORY = new ObjectFactory<Quaternion>() { 809 @Override 810 protected Quaternion create() { 811 return new Quaternion(); 812 } 813 }; 814 815 private static Quaternion newInstance() { 816 Quaternion Q = FACTORY.object(); 817 return Q; 818 } 819 820 private static Quaternion copyOf(Quaternion original) { 821 Quaternion Q = Quaternion.newInstance(); 822 Q._data = original._data.copy(); 823 return Q; 824 } 825 826 private Quaternion() {} 827 828 829 /** 830 * Tests the methods in this class. 831 * 832 * @param args the command-line arguments. 833 */ 834 public static void main (String args[]) { 835 System.out.println("Testing Quaternion:"); 836 837 Parameter<Angle> psi = Parameter.valueOf(60,NonSI.DEGREE_ANGLE); 838 Parameter<Angle> theta = Parameter.ZERO_ANGLE; 839 Parameter<Angle> phi = Parameter.ZERO_ANGLE; 840 System.out.println("psi = " + psi + ", theta = " + theta + ", phi = " + phi); 841 842 DCMatrix m1 = DCMatrix.getEulerTM(psi, theta, phi); 843 Quaternion q1 = Quaternion.valueOf(m1); 844 System.out.println("m1 = \n" + m1); 845 System.out.println("q1 = " + q1); 846 System.out.println("q1.norm() = " + q1.norm()); 847 System.out.println("q1.getVector() = " + q1.getVector()); 848 System.out.println("q1.getScalar() = " + q1.getScalar()); 849 System.out.println("q1.conjugate() = " + q1.conjugate()); 850 System.out.println("q1.inverse() = " + q1.inverse()); 851 System.out.println("q1.toDCM() = \n" + q1.toDCM()); 852 853 Vector3D<Length> v1 = Vector3D.valueOf(1, 0, 0, SI.METER); 854 System.out.println("\nv1 = " + v1); 855 System.out.println(" q1.transform(v1) = " + q1.transform(v1)); 856 857 Quaternion q2 = Quaternion.valueOf(Vector3D.UNIT_Z, psi); 858 System.out.println("q2 = (should == q1) = " + q2); 859 860 theta = Parameter.valueOf(30,NonSI.DEGREE_ANGLE); 861 DCMatrix m2 = DCMatrix.getEulerTM(psi, theta, phi); 862 Quaternion q3 = Quaternion.valueOf(Vector3D.UNIT_Y, theta); 863 Quaternion q4 = q1.times(q3); 864 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 865 System.out.println("m2 = \n" + m2); 866 System.out.println("q3 = " + q3); 867 System.out.println("q4 = q1.times(q3) = " + q4); 868 System.out.println(" q4.transform(v1) = " + q4.transform(v1)); 869 System.out.println(" m2.times(v1) = " + m2.times(v1)); 870 System.out.println("Lq1.times(q3) = " + q1.toLeftMatrix().times(q3.toFloat64Vector())); 871 System.out.println("Rq1.times(q3) = " + q1.toRightMatrix().times(q3.toFloat64Vector())); 872 System.out.println("q3.times(q1) = " + q3.times(q1)); 873 System.out.println("q4.conjugate() = " + q4.conjugate()); 874 Quaternion q4T = Quaternion.valueOf(m2.transpose()); 875 System.out.println("q4T = " + q4T); 876 System.out.println("m2.toQuaternion() = " + m2.toQuaternion()); 877 System.out.println("m2.transpose().toQuaternion() = " + m2.transpose().toQuaternion()); 878 DCMatrix m4 = q4.toDCM(); 879 System.out.println("\nq4 = " + q4); 880 System.out.println("m4 = q4.toDCM() = \n" + m4); 881 System.out.println("m4.toQuaternion() = " + m4.toQuaternion()); 882 System.out.println("q3.toUnit() = " + q3.toUnit()); 883 884 // Write out XML data. 885 try { 886 System.out.println(); 887 888 // Creates some useful aliases for class names. 889 javolution.xml.XMLBinding binding = new javolution.xml.XMLBinding(); 890 binding.setAlias(org.jscience.mathematics.number.Float64.class, "Float64"); 891 binding.setAlias(org.jscience.mathematics.vector.Float64Matrix.class, "Float64Matrix"); 892 893 javolution.xml.XMLObjectWriter writer = javolution.xml.XMLObjectWriter.newInstance(System.out); 894 writer.setIndentation(" "); 895 writer.setBinding(binding); 896 writer.write(q1, "Quaternion", Quaternion.class); 897 writer.flush(); 898 899 System.out.println(); 900 } catch (Exception e) { 901 e.printStackTrace(); 902 } 903 904 } 905 906}