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