001/* 002 * EulerAngles -- Three angles representing the orientation between two reference frames. 003 * 004 * Copyright (C) 2009-2025, by Joseph A. Huwaldt. All rights reserved. 005 * 006 * This library is free software; you can redistribute it and/or 007 * modify it under the terms of the GNU Lesser General Public 008 * License as published by the Free Software Foundation; either 009 * version 2 of the License, or (at your option) any later version. 010 * 011 * This library is distributed in the hope that it will be useful, 012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 014 * Lesser General Public License for more details. 015 * 016 * You should have received a copy of the GNU Lesser General Public License 017 * along with this program; if not, write to the Free Software 018 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 019 * Or visit: http://www.gnu.org/licenses/lgpl.html 020 */ 021package jahuwaldt.js.param; 022 023import java.text.MessageFormat; 024import javax.measure.converter.ConversionException; 025import javax.measure.quantity.Angle; 026import javax.measure.quantity.Length; 027import javax.measure.quantity.Quantity; 028import javax.measure.unit.NonSI; 029import javax.measure.unit.SI; 030import javax.measure.unit.Unit; 031import javolution.context.ImmortalContext; 032import javolution.context.ObjectFactory; 033import javolution.context.StackContext; 034import static javolution.lang.MathLib.*; 035import javolution.text.Text; 036import javolution.text.TextBuilder; 037import javolution.util.FastMap; 038import javolution.util.FastTable; 039import javolution.xml.XMLFormat; 040import javolution.xml.XMLSerializable; 041import javolution.xml.stream.XMLStreamException; 042import org.jscience.mathematics.number.Float64; 043import org.jscience.mathematics.vector.DimensionException; 044import org.jscience.mathematics.vector.Float64Vector; 045import org.jscience.mathematics.vector.Vector; 046 047/** 048 * This class represents the three Euler angles; theta1, theta2, theta3. Euler angles may 049 * be used to represents a relative attitude (attitude transformation or rotation) between 050 * two different reference frames; B wrt A or BA. It can be used to transform coordinates 051 * in reference frame A to reference frame B (A2B). 052 * <p> 053 * Reference: Shoemake, Ken (1994), "Euler angle conversion", in Paul Heckbert, Graphics 054 * Gems IV, San Diego: Academic Press Professional, pp. 222-229, ISBN 978-0-12-336155-4. 055 * </p> 056 * 057 * <p> Modified by: Joseph A. Huwaldt </p> 058 * 059 * @author Joseph A. Huwaldt, Date: February 4, 2009 060 * @version February 23, 2025 061 */ 062public final class EulerAngles extends AbstractRotation<EulerAngles> implements XMLSerializable { 063 064 private static final long serialVersionUID = 6575073905088777817L; 065 066 /** 067 * Enumeration of the axes that a rotation may be made around. 068 */ 069 public static enum Axis { 070 071 X, Y, Z 072 }; 073 074 /** 075 * Enumeration of the parity of the axis permutation. 076 */ 077 public static enum Parity { 078 079 EVEN, ODD 080 }; 081 082 /** 083 * Indicates that the initial axis is NOT repeated as the last axis. 084 */ 085 public static final boolean NO = false; 086 087 /** 088 * Indicates repetition of initial axis as the last. 089 */ 090 public static final boolean YES = true; 091 092 /** 093 * Enumeration of the frame from which the axes are taken. 094 */ 095 public static enum Frame { 096 097 STATIC, ROTATING 098 }; 099 100 /** 101 * Enumeration of all the combinations of the Euler angle orders for both static ("s") 102 * and rotating ("r") axes. 103 */ 104 public static enum Order { 105 106 // Static axes. 107 108 XYZs(Axis.X, Parity.EVEN, NO, Frame.STATIC), 109 XYXs(Axis.X, Parity.EVEN, YES, Frame.STATIC), 110 XZYs(Axis.X, Parity.ODD, NO, Frame.STATIC), 111 XZXs(Axis.X, Parity.ODD, YES, Frame.STATIC), 112 YZXs(Axis.Y, Parity.EVEN, NO, Frame.STATIC), 113 YZYs(Axis.Y, Parity.EVEN, YES, Frame.STATIC), 114 YXZs(Axis.Y, Parity.ODD, NO, Frame.STATIC), 115 YXYs(Axis.Y, Parity.ODD, YES, Frame.STATIC), 116 ZXYs(Axis.Z, Parity.EVEN, NO, Frame.STATIC), 117 ZXZs(Axis.Z, Parity.EVEN, YES, Frame.STATIC), 118 ZYXs(Axis.Z, Parity.ODD, NO, Frame.STATIC), 119 ZYZs(Axis.Z, Parity.ODD, YES, Frame.STATIC), 120 // Rotating axes. 121 ZYXr(Axis.X, Parity.EVEN, NO, Frame.ROTATING), 122 XYXr(Axis.X, Parity.EVEN, YES, Frame.ROTATING), 123 YZXr(Axis.X, Parity.ODD, NO, Frame.ROTATING), 124 XZXr(Axis.X, Parity.ODD, YES, Frame.ROTATING), 125 XZYr(Axis.Y, Parity.EVEN, NO, Frame.ROTATING), 126 YZYr(Axis.Y, Parity.EVEN, YES, Frame.ROTATING), 127 ZXYr(Axis.Y, Parity.ODD, NO, Frame.ROTATING), 128 YXYr(Axis.Y, Parity.ODD, YES, Frame.ROTATING), 129 YXZr(Axis.Z, Parity.EVEN, NO, Frame.ROTATING), 130 ZXZr(Axis.Z, Parity.EVEN, YES, Frame.ROTATING), 131 XYZr(Axis.Z, Parity.ODD, NO, Frame.ROTATING), 132 ZYZr(Axis.Z, Parity.ODD, YES, Frame.ROTATING); 133 134 // A table used to cross reference Order elements and their parts 135 // encoded as a String. 136 private static final FastMap<Text, Order> lookup; 137 138 static { 139 TextBuilder buf = TextBuilder.newInstance(); 140 ImmortalContext.enter(); 141 try { 142 lookup = FastMap.newInstance(); 143 144 // Fill in the lookup table. 145 for (Order order : Order.values()) { 146 buf.append(order.getInitialAxis()); 147 buf.append(order.getParity()); 148 buf.append(order.getRepeat1st()); 149 buf.append(order.getFrame()); 150 Text txt = buf.toText(); 151 lookup.put(txt, order); 152 buf.clear(); 153 } 154 } finally { 155 ImmortalContext.exit(); 156 } 157 } 158 159 private final Axis initialAxis; 160 private final Parity parity; 161 private final boolean rep; 162 private final Frame frame; 163 164 private Order(Axis initialAxis, Parity parity, boolean repeat1st, Frame frame) { 165 this.initialAxis = initialAxis; 166 this.parity = parity; 167 this.rep = repeat1st; 168 this.frame = frame; 169 } 170 171 /** 172 * Returns the initial rotation axis. 173 * 174 * @return the initial rotation axis. 175 */ 176 public Axis getInitialAxis() { 177 return initialAxis; 178 } 179 180 /** 181 * Returns the parity of the axis permutation. 182 * 183 * @return the parity of the axis permutation. 184 */ 185 public Parity getParity() { 186 return parity; 187 } 188 189 /** 190 * Returns true if the initial axis is repeated as the last and false if it is 191 * not. 192 * 193 * @return The value <code>true</code> if the initial axis is repeated as 194 * the last and <code>false</code> if it is not. 195 */ 196 public boolean getRepeat1st() { 197 return rep; 198 } 199 200 /** 201 * Returns the frame from which the axes are taken. 202 * 203 * @return The frame from which the axes are taken. 204 */ 205 public Frame getFrame() { 206 return frame; 207 } 208 209 /** 210 * Returns the Order that corresponds to the specified order parts. 211 * 212 * @param initialAxis The initial rotation axis (X, Y, or Z). 213 * @param parity The parity of the axis permutation (EVEN or ODD). 214 * @param repeat1st Repeat the initial axis as the last if true, do not repeat 215 * if false. 216 * @param frame The frame from which the axes are taken (STATIC or 217 * ROTATING). 218 * @return The Order that corresponds to the input order parts. 219 */ 220 public static Order valueOf(Axis initialAxis, Parity parity, boolean repeat1st, Frame frame) { 221 TextBuilder buf = TextBuilder.newInstance(); 222 buf.append(initialAxis); 223 buf.append(parity); 224 buf.append(repeat1st); 225 buf.append(frame); 226 Text txt = buf.toText(); 227 TextBuilder.recycle(buf); 228 return lookup.get(txt); 229 } 230 } 231 232 /** 233 * The Euler order typically used in aerospace applications. Also known as Tait-Bryan 234 * rotations. Yaw about Z axis, pitch about yawed Y axis, and finally roll about 235 * pitch-yawed X axis. 236 * 237 * @see 238 * <a href="https://en.wikipedia.org/wiki/Euler_angles#Tait.E2.80.93Bryan_angles"> 239 * Wikipedia: Euler Angles: Tait–Bryan angles</a> 240 */ 241 public static final Order YAW_PITCH_ROLL = Order.ZYXr; 242 243 // Floating point epsilon. 244 private static final double FLT_EPSILON = Parameter.EPS; 245 246 // The Euler Angle elements. 247 private Vector3D<Angle> _theta; 248 249 // The Euler axis order for this set of angles. 250 private Order _order; 251 252 /** 253 * Returns an {@link EulerAngles} instance holding the specified 254 * <code>Vector3D<Angle></code> values for the rotation angle elements and the 255 * specified Euler order information. 256 * 257 * @param theta The rotation angles about each axis (0=1st axis, 1=2nd axis, 258 * 2=3rd axis). 259 * @param initialAxis The initial rotation axis (X, Y, or Z). 260 * @param parity The parity of the axis permutation (EVEN or ODD). 261 * @param repeat1st Repeat the initial axis as the last if true, do not repeat if 262 * false. 263 * @param frame The frame from which the axes are taken (STATIC or ROTATING). 264 * @return The EulerAngles having the specified values. 265 */ 266 public static EulerAngles valueOf(Vector3D<Angle> theta, Axis initialAxis, Parity parity, boolean repeat1st, Frame frame) { 267 return valueOf(theta, Order.valueOf(initialAxis, parity, repeat1st, frame)); 268 } 269 270 /** 271 * Returns an {@link EulerAngles} instance holding the specified 272 * <code>Vector3D<Angle></code> values for the rotation angle elements and the 273 * specified Euler order information. 274 * 275 * @param theta The rotation angles about each axis (0=1st axis, 1=2nd axis, 276 * 2=3rd axis). 277 * @param eulerOrder The Euler order to use (one of the enumerations in this class). 278 * @return The EulerAngles having the specified values. 279 */ 280 public static EulerAngles valueOf(Vector3D<Angle> theta, Order eulerOrder) { 281 EulerAngles ea = EulerAngles.newInstance(eulerOrder); 282 ea._theta = theta; 283 return ea; 284 } 285 286 /** 287 * Returns an {@link EulerAngles} instance holding the specified 288 * <code>Parameter<Angle></code> values for the rotation angle elements and the 289 * specified Euler order information. 290 * 291 * @param theta1 The rotation angle about the 1st axis. 292 * @param theta2 The rotation angle about the 2nd axis. 293 * @param theta3 The rotation angle about the 3rd axis. 294 * @param eulerOrder The Euler order to use (one of the enumerations in this class). 295 * @return The EulerAngles having the specified values. 296 */ 297 public static EulerAngles valueOf(Parameter<Angle> theta1, Parameter<Angle> theta2, Parameter<Angle> theta3, Order eulerOrder) { 298 Vector3D<Angle> theta = Vector3D.valueOf(theta1, theta2, theta3); 299 return valueOf(theta, eulerOrder); 300 } 301 302 /** 303 * Returns an {@link EulerAngles} object constructed from the specified rotation 304 * transformation. 305 * 306 * @param transform The rotation transform representing a rotation from frame A to 307 * frame B. 308 * @param order The Euler order to use (one of the enumerations in this class). 309 * @return The Euler angles representing the specified direction cosine rotation 310 * matrix. 311 */ 312 public static EulerAngles valueOf(Rotation<?> transform, Order order) { 313 314 // Convert the rotation to a DCM. 315 DCMatrix dc = transform.toDCM(); 316 317 int i = getInitialAxisIndex(order); 318 int j = getMiddleAxisIndex(order); 319 int k = getLastAxisIndex(order); 320 321 double Mii = dc.getValue(i, i); 322 double Mji = dc.getValue(j, i); 323 double Mjj = dc.getValue(j, j); 324 double Mjk = dc.getValue(j, k); 325 double Mki = dc.getValue(k, i); 326 327 double eax, eay, eaz; 328 if (order.getRepeat1st() == YES) { 329 double Mij = dc.getValue(i, j); 330 double Mik = dc.getValue(i, k); 331 332 double sy = sqrt(Mij * Mij + Mik * Mik); 333 if (sy > 16 * FLT_EPSILON) { 334 eax = atan2(Mij, Mik); 335 eay = atan2(sy, Mii); 336 eaz = atan2(Mji, -Mki); 337 338 } else { 339 eax = atan2(-Mjk, Mjj); 340 eay = atan2(sy, Mii); 341 eaz = 0; 342 } 343 344 } else { 345 double Mkj = dc.getValue(k, j); 346 double Mkk = dc.getValue(k, k); 347 348 double cy = sqrt(Mii * Mii + Mji * Mji); 349 if (cy > 16 * FLT_EPSILON) { 350 eax = atan2(Mkj, Mkk); 351 eay = atan2(-Mki, cy); 352 eaz = atan2(Mji, Mii); 353 354 } else { 355 eax = atan2(-Mjk, Mjj); 356 eay = atan2(-Mki, cy); 357 eaz = 0; 358 } 359 } 360 361 if (order.getParity() == Parity.ODD) { 362 eax = -eax; 363 eay = -eay; 364 eaz = -eaz; 365 } 366 367 if (order.getFrame() == Frame.ROTATING) { 368 double t = eax; 369 eax = eaz; 370 eaz = t; 371 } 372 373 // Bound the angles to the range 0 to 2*PI. 374 eax = bound2Pi(eax); 375 eay = bound2Pi(eay); 376 eaz = bound2Pi(eaz); 377 378 Vector3D<Angle> theta = Vector3D.valueOf(eax, eay, eaz, SI.RADIAN); 379 return valueOf(theta, order); 380 } 381 382 /** 383 * Returns the input angle (in radians) bounded to the range 0 to 2*PI. 384 */ 385 private static double bound2Pi(double angle) { 386 while (angle > TWO_PI) 387 angle -= TWO_PI; 388 while (angle < 0) 389 angle += TWO_PI; 390 return angle; 391 } 392 393 /** 394 * Returns the value of an element from this set of Euler angles (0=1st axis, 1=2nd 395 * axis, 2=3rd axis). 396 * 397 * @param i the dimension index (0=1st axis, 1=2nd axis, 2=3rd axis). 398 * @return the value of the element at <code>i</code>. 399 * @throws IndexOutOfBoundsException <code>(i < 0) || (i > dimension()-1)</code> 400 */ 401 public Parameter<Angle> get(int i) { 402 return _theta.get(i); 403 } 404 405 /** 406 * Returns the value of a floating point number from this set of Euler angles (0=1st 407 * axis, 1=2nd axis, 2=3rd axis). This returns the value of the specified Euler angle 408 * in whatever the current units of the Euler angles are. 409 * 410 * @param i the floating point number index (0=1st axis, 1=2nd axis, 2=3rd axis). 411 * @return the value of the floating point number at <code>i</code>. 412 * @throws IndexOutOfBoundsException <code>(i < 0) || (i > dimension()-1)</code> 413 */ 414 public double getValue(int i) { 415 return _theta.getValue(i); 416 } 417 418 /** 419 * Returns the Euler order for this set of Euler angles. 420 * 421 * @return The Euler order for this set of Euler angles. 422 */ 423 public Order getEulerOrder() { 424 return _order; 425 } 426 427 /** 428 * Returns the negation of this set of Euler angles (each angle is negated). 429 * 430 * @return <code>-this</code>. 431 */ 432 public EulerAngles opposite() { 433 EulerAngles ea = EulerAngles.newInstance(_order); 434 ea._theta = _theta.opposite(); 435 return ea; 436 } 437 438 /** 439 * Returns the sum of this set of Euler angles with the one specified. The unit of the 440 * output set of Euler angles will be the units of this set of Euler angles. 441 * 442 * @param that the Euler angles to be added. 443 * @return <code>this + that</code>. 444 */ 445 public EulerAngles plus(EulerAngles that) { 446 447 EulerAngles ea = EulerAngles.newInstance(_order); 448 ea._theta = this._theta.plus(that._theta); 449 450 return ea; 451 } 452 453 /** 454 * Returns the sum of this set of Euler angles with the one specified. The unit of the 455 * output set of Euler angles will be the units of this set of Euler angles. 456 * 457 * @param that the set of Euler angles of angles to be added. 458 * @return <code>this + that</code>. 459 * @throws DimensionException if vector dimensions are different. 460 * @throws ConversionException if the current model does not allow for conversion the 461 * units of the input vector to an Angle unit. 462 */ 463 public EulerAngles plus(Vector<Parameter<Angle>> that) { 464 if (!that.get(0).getUnit().isCompatible(Angle.UNIT)) 465 throw new ConversionException(RESOURCES.getString("eaBadAngleUnits")); 466 467 // Convert input vector to a Vector3D object. 468 Vector3D<Angle> thatV = Vector3D.valueOf(that); 469 470 EulerAngles ea = EulerAngles.newInstance(_order); 471 ea._theta = this._theta.plus(thatV); 472 473 return ea; 474 } 475 476 /** 477 * Returns the sum of this set of Euler angles with the angle specified. The input 478 * parameter is added to each component of this set of Euler angles. The unit of the 479 * output Euler angles will be the units of this set of Euler angles. 480 * 481 * @param that the angle to be added to each element of this set of Euler angles. 482 * @return <code>this + that</code>. 483 */ 484 public EulerAngles plus(Parameter<Angle> that) { 485 486 EulerAngles ea = EulerAngles.newInstance(_order); 487 ea._theta = this._theta.plus(that); 488 489 return ea; 490 } 491 492 /** 493 * Returns the difference between this set of Euler angles and the one specified. 494 * 495 * @param that the Euler angles to be subtracted. 496 * @return <code>this - that</code>. 497 */ 498 public EulerAngles minus(EulerAngles that) { 499 500 EulerAngles ea = EulerAngles.newInstance(_order); 501 ea._theta = this._theta.minus(that._theta); 502 503 return ea; 504 } 505 506 /** 507 * Returns the difference between this set of Euler angles and the one specified. 508 * 509 * @param that the vector of angles to be subtracted. 510 * @return <code>this - that</code>. 511 * @throws DimensionException if vector dimensions are different. 512 * @throws ConversionException if the current model does not allow for conversion the 513 * units of the input vector to an Angle unit. 514 */ 515 public EulerAngles minus(Vector<Parameter<Angle>> that) { 516 if (!that.get(0).getUnit().isCompatible(Angle.UNIT)) 517 throw new ConversionException(RESOURCES.getString("eaBadAngleUnits")); 518 519 // Convert input vector to a Vector3D object. 520 Vector3D<Angle> thatV = Vector3D.valueOf(that); 521 522 EulerAngles ea = EulerAngles.newInstance(_order); 523 ea._theta = this._theta.minus(thatV); 524 525 return ea; 526 } 527 528 /** 529 * Subtracts the supplied angle from each element of this set of Euler angles and 530 * returns the result. The unit of the output Euler angles will be the units of this 531 * set of Euler angles. 532 * 533 * @param that the angle to be subtracted from each element of this set of Euler 534 * angles. 535 * @return <code>this - that</code>. 536 */ 537 public EulerAngles minus(Parameter<Angle> that) { 538 539 EulerAngles ea = EulerAngles.newInstance(_order); 540 ea._theta = this._theta.minus(that); 541 542 return ea; 543 } 544 545 /** 546 * Returns the product of this set of Euler angles with the specified coefficient. 547 * 548 * @param k the coefficient multiplier. 549 * @return <code>this · k</code> 550 */ 551 public EulerAngles times(double k) { 552 EulerAngles ea = EulerAngles.newInstance(_order); 553 ea._theta = this._theta.times(k); 554 return ea; 555 } 556 557 /** 558 * Returns this set of Euler angles with each angle divided by the specified divisor. 559 * 560 * @param divisor the divisor. 561 * @return <code>this / divisor</code>. 562 */ 563 public EulerAngles divide(double divisor) { 564 EulerAngles ea = EulerAngles.newInstance(_order); 565 ea._theta = _theta.divide(divisor); 566 return ea; 567 } 568 569 /** 570 * Transforms a 3D vector from frame A to B using this set of Euler angles. 571 * 572 * @param <Q> The Quantity (unit type) of the input and output vectors. 573 * @param v the vector expressed in frame A. 574 * @return the vector expressed in frame B. 575 */ 576 @Override 577 public <Q extends Quantity> Vector3D<Q> transform(Coordinate3D<Q> v) { 578 579 Quaternion q = toQuaternion(); 580 Vector3D<Q> out = q.transform(v); 581 582 return out; 583 } 584 585 /** 586 * Returns the spatial inverse of this transformation: AB rather than BA. 587 * 588 * @return <code>this'</code> 589 */ 590 @Override 591 public EulerAngles transpose() { 592 Quaternion qT = toQuaternion().transpose(); 593 return EulerAngles.valueOf(qT, getEulerOrder()); 594 } 595 596 /** 597 * Returns the product of this set of Euler angles and the specified rotation 598 * transform. If this set of Euler angles is BA and that is AC then the returned value 599 * is: BC = BA · AC (or C2B = A2B · C2A). 600 * 601 * @param that the rotation transform multiplier. 602 * @return <code>this · that</code> 603 */ 604 @Override 605 public EulerAngles times(Rotation<?> that) { 606 return EulerAngles.valueOf(this.toDCM().times(that), this.getEulerOrder()); 607 } 608 609 /** 610 * Returns the unit in which the {@link #getValue values} in this set of Euler angles 611 * are stated in. 612 * 613 * @return The angular unit in which the values in this set of Euler angles are 614 * stated. 615 */ 616 public Unit<Angle> getUnit() { 617 return _theta.getUnit(); 618 } 619 620 /** 621 * Returns the equivalent to this set of Euler angles but stated in the specified 622 * angle unit. 623 * 624 * @param unit the angle unit of the Euler angles to be returned. 625 * @return A set of Euler angles equivalent to this set but stated in the specified 626 * unit. 627 * @throws ConversionException if the current model does not allows for conversion to 628 * the specified unit. 629 */ 630 public EulerAngles to(Unit<Angle> unit) { 631 EulerAngles ea = EulerAngles.newInstance(_order); 632 ea._theta = _theta.to(unit); 633 return ea; 634 } 635 636 /** 637 * Returns the values stored in this set of Euler angles, in the current units, as a 638 * Float64Vector with the values ordered (1st axis angle, 2nd axis angle, 3rd axis 639 * angle). 640 * 641 * @return The values stored in this set of Euler angles as a Float64Vector. 642 */ 643 public Float64Vector toFloat64Vector() { 644 return _theta.toFloat64Vector(); 645 } 646 647 /** 648 * Returns a direction cosine transformation matrix from this set of Euler angles. 649 * 650 * @return a direction cosine matrix that converts from frame A to B. 651 * @see <a href="http://en.wikipedia.org/wiki/Rotation_matrix#EulerAngles"> 652 * Wikipedia: Rotation Matrix-EulerAngles</a> 653 */ 654 @Override 655 public DCMatrix toDCM() { 656 StackContext.enter(); 657 try { 658 double eax = _theta.get(Vector3D.X).getValue(SI.RADIAN); 659 double eay = _theta.get(Vector3D.Y).getValue(SI.RADIAN); 660 double eaz = _theta.get(Vector3D.Z).getValue(SI.RADIAN); 661 662 if (_order.getFrame() == Frame.ROTATING) { 663 double t = eax; 664 eax = eaz; 665 eaz = t; 666 } 667 668 if (_order.getParity() == Parity.ODD) { 669 eax = -eax; 670 eay = -eay; 671 eaz = -eaz; 672 } 673 674 double ti = eax, tj = eay, th = eaz; 675 double ci = cos(ti), cj = cos(tj), ch = cos(th); 676 double si = sin(ti), sj = sin(tj), sh = sin(th); 677 double cc = ci * ch, cs = ci * sh, sc = si * ch, ss = si * sh; 678 679 int i = getInitialAxisIndex(_order); 680 int j = getMiddleAxisIndex(_order); 681 int k = getLastAxisIndex(_order); 682 683 double[] a = QA_FACTORY.object(); 684 FastTable<Float64Vector> rows = FastTable.newInstance(); 685 rows.setSize(3); 686 if (_order.getRepeat1st() == YES) { 687 a[i] = cj; 688 a[j] = sj * si; 689 a[k] = sj * ci; 690 rows.set(i, Float64Vector.valueOf(a)); 691 692 a[i] = sj * sh; 693 a[j] = -cj * ss + cc; 694 a[k] = -cj * cs - sc; 695 rows.set(j, Float64Vector.valueOf(a)); 696 697 a[i] = -sj * ch; 698 a[j] = cj * sc + cs; 699 a[k] = cj * cc - ss; 700 rows.set(k, Float64Vector.valueOf(a)); 701 702 } else { 703 a[i] = cj * ch; 704 a[j] = sj * sc - cs; 705 a[k] = sj * cc + ss; 706 rows.set(i, Float64Vector.valueOf(a)); 707 708 a[i] = cj * sh; 709 a[j] = sj * ss + cc; 710 a[k] = sj * cs - sc; 711 rows.set(j, Float64Vector.valueOf(a)); 712 713 a[i] = -sj; 714 a[j] = cj * si; 715 a[k] = cj * ci; 716 rows.set(k, Float64Vector.valueOf(a)); 717 } 718 719 DCMatrix M = DCMatrix.valueOf(rows); 720 return StackContext.outerCopy(M); 721 722 } finally { 723 StackContext.exit(); 724 } 725 } 726 727 /** 728 * Returns a quaternion from this set of Euler angles. 729 * 730 * @return a quaternion that transforms from frame A to B. 731 * @see <a href="http://en.wikipedia.org/wiki/Quaternion"> 732 * Wikipedia: Quaternion</a> 733 */ 734 @Override 735 public Quaternion toQuaternion() { 736 StackContext.enter(); 737 try { 738 double eax = _theta.get(Vector3D.X).getValue(SI.RADIAN); 739 double eay = _theta.get(Vector3D.Y).getValue(SI.RADIAN); 740 double eaz = _theta.get(Vector3D.Z).getValue(SI.RADIAN); 741 742 if (_order.getFrame() == Frame.ROTATING) { 743 double t = eax; 744 eax = eaz; 745 eaz = t; 746 } 747 748 if (_order.getParity() == Parity.ODD) 749 eay = -eay; 750 751 double ti = 0.5 * eax; 752 double tj = 0.5 * eay; 753 double th = 0.5 * eaz; 754 755 double ci = cos(ti), cj = cos(tj), ch = cos(th); 756 double si = sin(ti), sj = sin(tj), sh = sin(th); 757 double cc = ci * ch, cs = ci * sh, sc = si * ch, ss = si * sh; 758 759 int i = getInitialAxisIndex(_order); 760 int j = getMiddleAxisIndex(_order); 761 int k = getLastAxisIndex(_order); 762 763 double[] a = QA_FACTORY.object(); 764 double qw; 765 if (_order.getRepeat1st() == YES) { 766 a[i] = cj * (cs + sc); 767 a[j] = sj * (cc + ss); 768 a[k] = sj * (cs - sc); 769 qw = cj * (cc - ss); 770 771 } else { 772 a[i] = cj * sc - sj * cs; 773 a[j] = cj * ss + sj * cc; 774 a[k] = cj * cs - sj * sc; 775 qw = cj * cc + sj * ss; 776 } 777 778 if (_order.getParity() == Parity.ODD) 779 a[j] = -a[j]; 780 781 Quaternion Q = Quaternion.valueOf(a[Quaternion.X], a[Quaternion.Y], a[Quaternion.Z], qw); 782 return StackContext.outerCopy(Q); 783 784 } finally { 785 StackContext.exit(); 786 } 787 } 788 789 /** 790 * Return the index to the 1st (initial) axis (0=X, 1=Y, 2=Z). 791 */ 792 private static int getInitialAxisIndex(Order order) { 793 return order.getInitialAxis().ordinal(); 794 } 795 796 private static final int[] next = {1, 2, 0, 1}; 797 798 /** 799 * Return the index to the 2nd (middle) axis (0=X, 1=Y, 2=Z). 800 */ 801 private static int getMiddleAxisIndex(Order order) { 802 int i = getInitialAxisIndex(order); 803 if (order.getParity() == Parity.ODD) 804 ++i; 805 return next[i]; 806 } 807 808 /** 809 * Return the index to the 3rd (last) axis (0=X, 1=Y, 2=Z). 810 */ 811 private static int getLastAxisIndex(Order order) { 812 int i = getInitialAxisIndex(order); 813 if (order.getParity() != Parity.ODD) 814 ++i; 815 return next[i]; 816 } 817 818 /** 819 * Returns a copy of this set of Euler angles 820 * {@link javolution.context.AllocatorContext allocated} by the calling thread 821 * (possibly on the stack). 822 * 823 * @return an identical and independent copy of this set of Euler angles. 824 */ 825 @Override 826 public EulerAngles copy() { 827 return copyOf(this); 828 } 829 830 /** 831 * Returns the text representation of this set of Euler angles. 832 * 833 * @return the text representation of this set of Euler angles. 834 */ 835 @Override 836 public Text toText() { 837 final int dimension = 3; 838 TextBuilder tmp = TextBuilder.newInstance(); 839 if (this.isApproxEqual(IDENTITY)) 840 tmp.append("{IDENTITY}"); 841 else { 842 tmp.append('{'); 843 for (int i = 0; i < dimension; i++) { 844 tmp.append(get(i)); 845 tmp.append(", "); 846 } 847 tmp.append(_order); 848 tmp.append('}'); 849 } 850 Text txt = tmp.toText(); 851 TextBuilder.recycle(tmp); 852 return txt; 853 } 854 855 /** 856 * Compares this EulerAngles against the specified object for strict equality (same 857 * rotation type and same values). 858 * 859 * @param that the object to compare with. 860 * @return <code>true</code> if this rotation is identical to that rotation; 861 * <code>false</code> otherwise. 862 */ 863 @Override 864 public boolean equals(Object that) { 865 if (this == that) 866 return true; 867 if ((that == null) || (that.getClass() != this.getClass())) 868 return false; 869 870 EulerAngles m = (EulerAngles)that; 871 if (!_order.equals(m._order)) 872 return false; 873 874 return _theta.equals(m._theta); 875 } 876 877 /** 878 * Returns the hash code for this rotation. 879 * 880 * @return the hash code value. 881 */ 882 @Override 883 public int hashCode() { 884 int hash = 7; 885 886 int var_code = _order.hashCode(); 887 hash = hash * 31 + var_code; 888 889 var_code = _theta.hashCode(); 890 hash = hash * 31 + var_code; 891 892 return hash; 893 } 894 895 /** 896 * Holds the default XML representation. For example: 897 * <pre> 898 * <EulerAngles unit = "rad" order = "ZYXr"> 899 * <X value="0.349" /> 900 * <Y value="6.11" /> 901 * <Z value="0.524" /> 902 * </EulerAngles> 903 * </pre> 904 */ 905 protected static final XMLFormat<EulerAngles> XML = new XMLFormat<EulerAngles>(EulerAngles.class) { 906 907 @Override 908 public EulerAngles newInstance(Class<EulerAngles> cls, InputElement xml) throws XMLStreamException { 909 return FACTORY.object(); 910 } 911 912 @SuppressWarnings("unchecked") 913 @Override 914 public void read(InputElement xml, EulerAngles ea) throws XMLStreamException { 915 916 Unit<Angle> unit = (Unit<Angle>)Unit.valueOf(xml.getAttribute("unit")); 917 String orderStr = xml.getAttribute("order").toString(); 918 try { 919 920 Order order = Order.valueOf(orderStr); 921 ea._order = order; 922 923 } catch (IllegalArgumentException e) { 924 throw new XMLStreamException(MessageFormat.format(RESOURCES.getString("eaBadOrder"), orderStr)); 925 } catch (NullPointerException e) { 926 throw new XMLStreamException(RESOURCES.getString("eaNoOrderAttribute")); 927 } 928 929 double x = xml.get("Theta1", Float64.class).doubleValue(); 930 double y = xml.get("Theta2", Float64.class).doubleValue(); 931 double z = xml.get("Theta3", Float64.class).doubleValue(); 932 ea._theta = Vector3D.valueOf(x, y, z, unit); 933 934 if (xml.hasNext()) 935 throw new XMLStreamException(RESOURCES.getString("toManyXMLElementsErr")); 936 } 937 938 @Override 939 public void write(EulerAngles ea, OutputElement xml) throws XMLStreamException { 940 941 xml.setAttribute("unit", ea.getUnit().toString()); 942 xml.setAttribute("order", ea.getEulerOrder().toString()); 943 xml.add(Float64.valueOf(ea._theta.getValue(Vector3D.X)), "Theta1", Float64.class); 944 xml.add(Float64.valueOf(ea._theta.getValue(Vector3D.Y)), "Theta2", Float64.class); 945 xml.add(Float64.valueOf(ea._theta.getValue(Vector3D.Z)), "Theta3", Float64.class); 946 947 } 948 }; 949 950 /////////////////////// 951 // Factory creation. // 952 /////////////////////// 953 private static final ObjectFactory<EulerAngles> FACTORY = new ObjectFactory<EulerAngles>() { 954 @Override 955 protected EulerAngles create() { 956 return new EulerAngles(); 957 } 958 }; 959 960 private static EulerAngles newInstance(Order order) { 961 EulerAngles ea = FACTORY.object(); 962 ea._order = order; 963 return ea; 964 } 965 966 private static EulerAngles copyOf(EulerAngles original) { 967 EulerAngles ea = EulerAngles.newInstance(original.getEulerOrder()); 968 ea._theta = original._theta.copy(); 969 return ea; 970 } 971 972 private EulerAngles() { 973 } 974 975 private static final ObjectFactory<double[]> QA_FACTORY = new ObjectFactory<double[]>() { 976 @Override 977 protected double[] create() { 978 return new double[3]; 979 } 980 }; 981 982 /** 983 * Tests the methods in this class. 984 * 985 * @param args Command line arguments (ignored) 986 */ 987 public static void main(String args[]) { 988 System.out.println("Testing EulerAngles:"); 989 990 Parameter<Angle> psi = Parameter.valueOf(60, NonSI.DEGREE_ANGLE); 991 Parameter<Angle> theta = Parameter.valueOf(30, NonSI.DEGREE_ANGLE); 992 Parameter<Angle> phi = Parameter.valueOf(-10, NonSI.DEGREE_ANGLE); 993 System.out.println("psi = " + psi + ", theta = " + theta + ", phi = " + phi); 994 995 EulerAngles ea1 = EulerAngles.valueOf(psi, theta, phi, YAW_PITCH_ROLL); 996 DCMatrix m1 = DCMatrix.getEulerTM(psi, theta, phi); 997 Quaternion q1 = Quaternion.valueOf(m1); 998 System.out.println("ea1 = " + ea1); 999 System.out.println("ea1.toDCM() = \n" + ea1.toDCM()); 1000 System.out.println("m1 = \n" + m1); 1001 System.out.println("ea1.toQuaternion() = " + ea1.toQuaternion()); 1002 System.out.println("q1 = " + q1); 1003 1004 Vector3D<Length> v1 = Vector3D.valueOf(1, 1, 1, SI.METER); 1005 System.out.println("v1 = " + v1); 1006 System.out.println("ea1.transform(v1) = " + ea1.transform(v1)); 1007 System.out.println("q1.transform(v1) = " + q1.transform(v1)); 1008 System.out.println("m1.times(v1) = " + m1.times(v1)); 1009 EulerAngles ea1T = EulerAngles.valueOf(m1.transpose(), YAW_PITCH_ROLL); 1010 System.out.println("ea1 = " + ea1); 1011 System.out.println("ea1T = " + ea1T.to(NonSI.DEGREE_ANGLE)); 1012 System.out.println("ea1.transpose() = " + ea1.transpose().to(NonSI.DEGREE_ANGLE)); 1013 1014 psi = Parameter.valueOf(20, NonSI.DEGREE_ANGLE); 1015 theta = Parameter.valueOf(-10, NonSI.DEGREE_ANGLE); 1016 phi = Parameter.valueOf(30, NonSI.DEGREE_ANGLE); 1017 System.out.println("\npsi = " + psi + ", theta = " + theta + ", phi = " + phi); 1018 DCMatrix m2 = DCMatrix.getEulerTM(psi, theta, phi); 1019 EulerAngles ea2 = EulerAngles.valueOf(m2, YAW_PITCH_ROLL); 1020 Quaternion q2 = Quaternion.valueOf(m2); 1021 EulerAngles ea3 = EulerAngles.valueOf(q2, YAW_PITCH_ROLL); 1022 System.out.println("m2 = \n" + m2); 1023 System.out.println("q2 = " + q2); 1024 System.out.println("ea2 (from m2) = " + ea2.to(NonSI.DEGREE_ANGLE)); 1025 System.out.println("ea3 (from q2) = " + ea3.to(NonSI.DEGREE_ANGLE)); 1026 1027 // Write out XML data. 1028 try { 1029 System.out.println(); 1030 1031 // Creates some useful aliases for class names. 1032 javolution.xml.XMLBinding binding = new javolution.xml.XMLBinding(); 1033 //binding.setAlias(org.jscience.mathematics.number.Float64.class, "Float64"); 1034 1035 javolution.xml.XMLObjectWriter writer = javolution.xml.XMLObjectWriter.newInstance(System.out); 1036 writer.setIndentation(" "); 1037 writer.setBinding(binding); 1038 writer.write(ea1, "EulerAngles", EulerAngles.class); 1039 writer.flush(); 1040 1041 System.out.println(); 1042 } catch (Exception e) { 1043 e.printStackTrace(); 1044 } 1045 1046 } 1047 1048}