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