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-2023 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
19 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
20 * </p>
21 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
22 * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
23 */
24 public class Angle3d implements Serializable
25 {
26 /** */
27 private static final long serialVersionUID = 20160000L;
28
29 /** The rotations of the rotation in 3D (RPY coded). */
30 private final AngleVector rotation;
31
32 /**
33 * @param rotation AngleVector; the angles of the rotation in 3D (RPY coded)
34 * @throws ValueRuntimeException in case the vector does not have exactly three elements
35 */
36 public Angle3d(final AngleVector rotation) throws ValueRuntimeException
37 {
38 if (rotation.size() != 3)
39 {
40 throw new ValueRuntimeException("Size of an RPY-rotation vector should be exactly 3. Got: " + rotation);
41 }
42 this.rotation = rotation;
43 }
44
45 /**
46 * @param roll Angle; (phi) the rotation around the x-axis
47 * @param pitch Angle; (theta) the rotation around the y-axis
48 * @param yaw Angle; (psi) the rotation around the z-axis
49 * @throws ValueRuntimeException in case the units are incorrect
50 */
51 public Angle3d(final Angle roll, final Angle pitch, final Angle yaw) throws ValueRuntimeException
52 {
53 this.rotation = DoubleVector.instantiate(new Angle[] {roll, pitch, yaw}, AngleUnit.SI, StorageType.DENSE);
54 }
55
56 /**
57 * @param roll double; (phi) the rotation around the x-axis
58 * @param pitch double; (theta) the rotation around the y-axis
59 * @param yaw double; (psi) the rotation around the z-axis
60 * @param unit AngleUnit; the unit of the RPY parameters
61 * @throws ValueRuntimeException in case the units are incorrect
62 */
63 public Angle3d(final double roll, final double pitch, final double yaw, final AngleUnit unit) throws ValueRuntimeException
64 {
65 this.rotation = DoubleVector.instantiate(new double[] {roll, pitch, yaw}, unit, StorageType.DENSE);
66 }
67
68 /**
69 * @return the roll.
70 */
71 public final Angle getRoll()
72 {
73 try
74 {
75 return this.rotation.get(0);
76 }
77 catch (ValueRuntimeException exception)
78 {
79 // should be impossible as we constructed the vector always with three elements
80 throw new RuntimeException(
81 "getRoll() gave an exception; apparently vector " + this.rotation + " was not constructed right",
82 exception);
83 }
84 }
85
86 /**
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 RuntimeException(
99 "getPitch() gave an exception; apparently vector " + this.rotation + " was not constructed right",
100 exception);
101 }
102 }
103
104 /**
105 * @return the yaw.
106 */
107 public final Angle getYaw()
108 {
109 try
110 {
111 return this.rotation.get(2);
112 }
113 catch (ValueRuntimeException exception)
114 {
115 // should be impossible as we constructed the vector always with three elements
116 throw new RuntimeException(
117 "getYaw() gave an exception; apparently vector " + this.rotation + " was not constructed right", exception);
118 }
119 }
120
121 /** {@inheritDoc} */
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 }