1 package org.opentrafficsim.core.math;
2
3 import java.util.Locale;
4
5 import org.djunits.unit.AngleUnit;
6 import org.djunits.value.StorageType;
7 import org.djunits.value.ValueException;
8 import org.djunits.value.vdouble.scalar.Angle;
9 import org.djunits.value.vdouble.vector.AngleVector;
10
11 /**
12 * 3D-rotation, RPY coded (longitudinal roll along the x-axis, lateral pitch along the y-axis and vertical yaw along the
13 * z-axis), also called Tait–Bryan angles o0r Cardan angles.
14 * <p>
15 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
16 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
17 * </p>
18 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
19 * initial version Dec 10, 2015 <br>
20 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
21 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
22 */
23 public interface Rotation3D
24 {
25 /**
26 * The absolute RPY coded 3D-rotation. Angles are absolute and relate to the absolute XYZ-frame of the world.
27 * <p>
28 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
29 * All rights reserved. <br>
30 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
31 * </p>
32 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
33 * initial version Dec 10, 2015 <br>
34 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
35 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
36 */
37 class Abs
38 {
39 /** The angles of the rotation in 3D (RPY coded). */
40 private final AngleVector.Abs rotation;
41
42 /**
43 * @param rotation the angles in 3D (RPY coded)
44 * @throws ValueException in case the vector does not have exactly three elements
45 */
46 public Abs(final AngleVector.Abs rotation) throws ValueException
47 {
48 super();
49 if (rotation.size() != 3)
50 {
51 throw new ValueException("Size of an RPY-rotation vector should be exactly 3. Got: " + rotation);
52 }
53 this.rotation = rotation;
54 }
55
56 /**
57 * @param roll (phi) the rotation around the x-axis
58 * @param pitch (theta) the rotation around the y-axis
59 * @param yaw (psi) the rotation around the z-axis
60 * @throws ValueException in case the units are incorrect
61 */
62 public Abs(final Angle.Abs roll, final Angle.Abs pitch, final Angle.Abs yaw) throws ValueException
63 {
64 super();
65 this.rotation = new AngleVector.Abs(new Angle.Abs[]{roll, pitch, yaw}, StorageType.DENSE);
66 }
67
68 /**
69 * @param roll (phi) the rotation around the x-axis
70 * @param pitch (theta) the rotation around the y-axis
71 * @param yaw (psi) the rotation around the z-axis
72 * @param unit the unit of the RPY parameters
73 * @throws ValueException in case the units are incorrect
74 */
75 public Abs(final double roll, final double pitch, final double yaw, final AngleUnit unit) throws ValueException
76 {
77 super();
78 this.rotation = new AngleVector.Abs(new double[]{roll, pitch, yaw}, unit, StorageType.DENSE);
79 }
80
81 /**
82 * @return the roll.
83 */
84 public final Angle.Abs getRoll()
85 {
86 try
87 {
88 return this.rotation.get(0);
89 }
90 catch (ValueException exception)
91 {
92 // should be impossible as we constructed the vector always with three elements
93 throw new RuntimeException("getRoll() gave an exception; apparently vector " + this.rotation
94 + " was not constructed right", exception);
95 }
96 }
97
98 /**
99 * @return the pitch.
100 */
101 public final Angle.Abs getPitch()
102 {
103 try
104 {
105 return this.rotation.get(1);
106 }
107 catch (ValueException exception)
108 {
109 // should be impossible as we constructed the vector always with three elements
110 throw new RuntimeException("getPitch() gave an exception; apparently vector " + this.rotation
111 + " was not constructed right", exception);
112 }
113 }
114
115 /**
116 * @return the yaw.
117 */
118 public final Angle.Abs getYaw()
119 {
120 try
121 {
122 return this.rotation.get(2);
123 }
124 catch (ValueException exception)
125 {
126 // should be impossible as we constructed the vector always with three elements
127 throw new RuntimeException("getYaw() gave an exception; apparently vector " + this.rotation
128 + " was not constructed right", exception);
129 }
130 }
131
132 /** {@inheritDoc} */
133 public final String toString()
134 {
135 return String
136 .format(Locale.US, "Rotation3D.Abs roll %s, pitch %s, yaw %s", getRoll(), getPitch(), getYaw());
137 }
138
139 }
140
141 /**
142 * The relative RPY coded 3D-rotation. Angles are relative, and can relate to e.g. the inertial frame of a GTU.
143 * <p>
144 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
145 * All rights reserved. <br>
146 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
147 * </p>
148 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
149 * initial version Dec 10, 2015 <br>
150 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
151 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
152 */
153 class Rel
154 {
155 /** The rotations of the rotation in 3D (RPY coded). */
156 private final AngleVector.Rel rotation;
157
158 /**
159 * @param rotation the angles of the rotation in 3D (RPY coded)
160 * @throws ValueException in case the vector does not have exactly three elements
161 */
162 public Rel(final AngleVector.Rel rotation) throws ValueException
163 {
164 super();
165 if (rotation.size() != 3)
166 {
167 throw new ValueException("Size of an RPY-rotation vector should be exactly 3. Got: " + rotation);
168 }
169 this.rotation = rotation;
170 }
171
172 /**
173 * @param roll (phi) the rotation around the x-axis
174 * @param pitch (theta) the rotation around the y-axis
175 * @param yaw (psi) the rotation around the z-axis
176 * @throws ValueException in case the units are incorrect
177 */
178 public Rel(final Angle.Rel roll, final Angle.Rel pitch, final Angle.Rel yaw) throws ValueException
179 {
180 super();
181 this.rotation = new AngleVector.Rel(new Angle.Rel[]{roll, pitch, yaw}, StorageType.DENSE);
182 }
183
184 /**
185 * @param roll (phi) the rotation around the x-axis
186 * @param pitch (theta) the rotation around the y-axis
187 * @param yaw (psi) the rotation around the z-axis
188 * @param unit the unit of the RPY parameters
189 * @throws ValueException in case the units are incorrect
190 */
191 public Rel(final double roll, final double pitch, final double yaw, final AngleUnit unit) throws ValueException
192 {
193 super();
194 this.rotation = new AngleVector.Rel(new double[]{roll, pitch, yaw}, unit, StorageType.DENSE);
195 }
196
197 /**
198 * @return the roll.
199 */
200 public final Angle.Rel getRoll()
201 {
202 try
203 {
204 return this.rotation.get(0);
205 }
206 catch (ValueException exception)
207 {
208 // should be impossible as we constructed the vector always with three elements
209 throw new RuntimeException("getRoll() gave an exception; apparently vector " + this.rotation
210 + " was not constructed right", exception);
211 }
212 }
213
214 /**
215 * @return the pitch.
216 */
217 public final Angle.Rel getPitch()
218 {
219 try
220 {
221 return this.rotation.get(1);
222 }
223 catch (ValueException exception)
224 {
225 // should be impossible as we constructed the vector always with three elements
226 throw new RuntimeException("getPitch() gave an exception; apparently vector " + this.rotation
227 + " was not constructed right", exception);
228 }
229 }
230
231 /**
232 * @return the yaw.
233 */
234 public final Angle.Rel getYaw()
235 {
236 try
237 {
238 return this.rotation.get(2);
239 }
240 catch (ValueException exception)
241 {
242 // should be impossible as we constructed the vector always with three elements
243 throw new RuntimeException("getYaw() gave an exception; apparently vector " + this.rotation
244 + " was not constructed right", exception);
245 }
246 }
247
248 /** {@inheritDoc} */
249 public final String toString()
250 {
251 return String
252 .format(Locale.US, "Rotation3D.Rel roll %s, pitch %s, yaw %s", getRoll(), getPitch(), getYaw());
253 }
254
255 }
256
257 }