1 package org.opentrafficsim.core.math;
2
3 import java.io.Serializable;
4 import java.util.Locale;
5
6 import org.djunits.unit.AngleUnit;
7 import org.djunits.value.ValueRuntimeException;
8 import org.djunits.value.storage.StorageType;
9 import org.djunits.value.vdouble.scalar.Angle;
10 import org.djunits.value.vdouble.vector.AngleVector;
11 import org.djunits.value.vdouble.vector.base.DoubleVector;
12
13 /**
14 * 3D-rotation, RPY coded (longitudinal roll along the x-axis, lateral pitch along the y-axis and vertical yaw along the
15 * z-axis), also called Tait–Bryan angles or Cardan angles. Angles are relative, and can relate to e.g. the inertial frame of a
16 * GTU.
17 * <p>
18 * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
19 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
20 * </p>
21 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
22 * initial version Dec 10, 2015 <br>
23 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
24 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
25 */
26 public class Angle3D implements Serializable
27 {
28 /** */
29 private static final long serialVersionUID = 20160000L;
30
31 /** The rotations of the rotation in 3D (RPY coded). */
32 private final AngleVector rotation;
33
34 /**
35 * @param rotation AngleVector; the angles of the rotation in 3D (RPY coded)
36 * @throws ValueRuntimeException in case the vector does not have exactly three elements
37 */
38 public Angle3D(final AngleVector rotation) throws ValueRuntimeException
39 {
40 if (rotation.size() != 3)
41 {
42 throw new ValueRuntimeException("Size of an RPY-rotation vector should be exactly 3. Got: " + rotation);
43 }
44 this.rotation = rotation;
45 }
46
47 /**
48 * @param roll Angle; (phi) the rotation around the x-axis
49 * @param pitch Angle; (theta) the rotation around the y-axis
50 * @param yaw Angle; (psi) the rotation around the z-axis
51 * @throws ValueRuntimeException in case the units are incorrect
52 */
53 public Angle3D(final Angle roll, final Angle pitch, final Angle yaw) throws ValueRuntimeException
54 {
55 this.rotation = DoubleVector.instantiate(new Angle[] {roll, pitch, yaw}, AngleUnit.SI, StorageType.DENSE);
56 }
57
58 /**
59 * @param roll double; (phi) the rotation around the x-axis
60 * @param pitch double; (theta) the rotation around the y-axis
61 * @param yaw double; (psi) the rotation around the z-axis
62 * @param unit AngleUnit; the unit of the RPY parameters
63 * @throws ValueRuntimeException in case the units are incorrect
64 */
65 public Angle3D(final double roll, final double pitch, final double yaw, final AngleUnit unit) throws ValueRuntimeException
66 {
67 this.rotation = DoubleVector.instantiate(new double[] {roll, pitch, yaw}, unit, StorageType.DENSE);
68 }
69
70 /**
71 * @return the roll.
72 */
73 public final Angle getRoll()
74 {
75 try
76 {
77 return this.rotation.get(0);
78 }
79 catch (ValueRuntimeException exception)
80 {
81 // should be impossible as we constructed the vector always with three elements
82 throw new RuntimeException(
83 "getRoll() gave an exception; apparently vector " + this.rotation + " was not constructed right",
84 exception);
85 }
86 }
87
88 /**
89 * @return the pitch.
90 */
91 public final Angle getPitch()
92 {
93 try
94 {
95 return this.rotation.get(1);
96 }
97 catch (ValueRuntimeException exception)
98 {
99 // should be impossible as we constructed the vector always with three elements
100 throw new RuntimeException(
101 "getPitch() gave an exception; apparently vector " + this.rotation + " was not constructed right",
102 exception);
103 }
104 }
105
106 /**
107 * @return the yaw.
108 */
109 public final Angle getYaw()
110 {
111 try
112 {
113 return this.rotation.get(2);
114 }
115 catch (ValueRuntimeException exception)
116 {
117 // should be impossible as we constructed the vector always with three elements
118 throw new RuntimeException(
119 "getYaw() gave an exception; apparently vector " + this.rotation + " was not constructed right", exception);
120 }
121 }
122
123 /** {@inheritDoc} */
124 @Override
125 public final String toString()
126 {
127 return String.format(Locale.US, "Angle3D.Rel roll %s, pitch %s, yaw %s", getRoll(), getPitch(), getYaw());
128 }
129 }