1 package org.opentrafficsim.core.math;
2
3 import java.util.Locale;
4
5 import org.djunits.unit.DirectionUnit;
6 import org.djunits.value.ValueRuntimeException;
7 import org.djunits.value.vdouble.scalar.Direction;
8 import org.djunits.value.vdouble.vector.DirectionVector;
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 absolute and relate to the absolute XYZ-frame of the
14 * world.
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 Direction3d
23 {
24 /** The angles of the rotation in 3D (RPY coded). */
25 private final DirectionVector rotation;
26
27 /**
28 * Constructor.
29 * @param rotation the angles in 3D (RPY coded)
30 * @throws ValueRuntimeException in case the vector does not have exactly three elements
31 */
32 public Direction3d(final DirectionVector 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 Direction3d(final Direction roll, final Direction pitch, final Direction yaw) throws ValueRuntimeException
49 {
50 this.rotation = new DirectionVector(new Direction[] {roll, pitch, yaw}, roll.getDisplayUnit());
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 Direction3d(final double roll, final double pitch, final double yaw, final DirectionUnit unit)
62 throws ValueRuntimeException
63 {
64 this.rotation = new DirectionVector(new double[] {roll, pitch, yaw}, unit);
65 }
66
67 /**
68 * Returns the roll.
69 * @return the roll.
70 */
71 public final Direction 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 OtsRuntimeException(
81 "getRoll() gave an exception; apparently vector " + this.rotation + " was not constructed right",
82 exception);
83 }
84 }
85
86 /**
87 * Returns the pitch.
88 * @return the pitch.
89 */
90 public final Direction getPitch()
91 {
92 try
93 {
94 return this.rotation.get(1);
95 }
96 catch (ValueRuntimeException exception)
97 {
98 // should be impossible as we constructed the vector always with three elements
99 throw new OtsRuntimeException(
100 "getPitch() gave an exception; apparently vector " + this.rotation + " was not constructed right",
101 exception);
102 }
103 }
104
105 /**
106 * Returns the yaw.
107 * @return the yaw.
108 */
109 public final Direction 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 OtsRuntimeException(
119 "getYaw() gave an exception; apparently vector " + this.rotation + " was not constructed right", exception);
120 }
121 }
122
123 @Override
124 public final String toString()
125 {
126 return String.format(Locale.US, "Rotation3d.Abs roll %s, pitch %s, yaw %s", getRoll(), getPitch(), getYaw());
127 }
128 }