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