View Javadoc
1   package org.djutils.draw.point;
2   
3   import java.util.Arrays;
4   import java.util.Iterator;
5   import java.util.Locale;
6   
7   import org.djutils.draw.Oriented3d;
8   import org.djutils.exceptions.Throw;
9   import org.djutils.math.AngleUtil;
10  
11  /**
12   * A OrientedPoint3d is a point with an x, y, and z coordinate, plus a 3d orientation. The orientation is specified by the
13   * rotations around the x, y, and z-axis. A number of constructors and methods are provided for cases where only the rotation
14   * around the z-axis is of importance. Orientation in 3D is stored as three double values dirX,dirY,dirZ. This class does
15   * <b>not</b> prescribe a particular order in which these rotations are to be applied. (Applying rotations is <b>not</b>
16   * commutative, so this <i>is</i> important.)
17   * <p>
18   * Copyright (c) 2020-2025 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
19   * BSD-style license. See <a href="https://djutils.org/docs/current/djutils/licenses.html">DJUTILS License</a>.
20   * </p>
21   * @author <a href="https://www.tudelft.nl/averbraeck">Alexander Verbraeck</a>
22   * @author <a href="https://github.com/peter-knoppers">Peter Knoppers</a>
23   * @author <a href="https://github.com/wjschakel">Wouter Schakel</a>
24   */
25  public class OrientedPoint3d extends DirectedPoint3d implements Oriented3d
26  {
27      /** */
28      private static final long serialVersionUID = 20200828L;
29  
30      /** The direction as rotation around the x-axis. */
31      @SuppressWarnings("checkstyle:visibilitymodifier")
32      public final double dirX;
33  
34      /**
35       * Create a new OrientedPoint3d with x, y, and z coordinates and direction 0,0,0.
36       * @param x the x coordinate
37       * @param y the y coordinate
38       * @param z the z coordinate
39       * @throws IllegalArgumentException when <code>x</code>, <code>y</code>, or <code>z</code> is <code>NaN</code>
40       */
41      public OrientedPoint3d(final double x, final double y, final double z)
42      {
43          this(x, y, z, 0.0, 0.0, 0.0);
44      }
45  
46      /**
47       * Create a new OrientedPoint3d with x, y, and z coordinates and orientation dirX,dirY,dirZ.
48       * @param x the x coordinate
49       * @param y the y coordinate
50       * @param z the z coordinate
51       * @param dirX the direction as rotation around the x-axis with the point as the center
52       * @param dirY the direction as rotation around the y-axis with the point as the center
53       * @param dirZ the direction as rotation around the z-axis with the point as the center
54       * @throws ArithmeticException when <code>x</code>, <code>y</code>, <code>z</code>, <code>dirX</code>, <code>dirY</code>, or
55       *             <code>dirZ</code> is <code>NaN</code>
56       */
57      public OrientedPoint3d(final double x, final double y, final double z, final double dirX, final double dirY,
58              final double dirZ)
59      {
60          super(x, y, z, dirY, dirZ);
61          Throw.whenNaN(dirX, "dirX");
62          this.dirX = dirX;
63      }
64  
65      /**
66       * Create a new OrientedPoint3d with x, y, and z coordinates and direction 0,0,0.
67       * @param xyz the x, y and z coordinates
68       * @throws NullPointerException when <code>xyz</code> is <code>null</code>
69       * @throws IllegalArgumentException when the length of the <code>xyx</code> array is not 3
70       * @throws IllegalArgumentException when the <code>xyx</code> array contains a <code>NaN</code> value
71       */
72      public OrientedPoint3d(final double[] xyz)
73      {
74          super(xyz, 0, 0);
75          this.dirX = 0.0;
76      }
77  
78      /**
79       * Create a new OrientedPoint3d with x, y, and z coordinates and orientation dirX,dirY,dirZ.
80       * @param xyz the x, y and z coordinates
81       * @param dirX the direction as rotation around the x-axis with the point as the center
82       * @param dirY the direction as rotation around the y-axis with the point as the center
83       * @param dirZ the direction as rotation around the z-axis with the point as the center
84       * @throws NullPointerException when <code>xyx</code> is <code>null</code>
85       * @throws IllegalArgumentException when the length of the xyz array is not 3
86       * @throws ArithmeticException when <code>xyz</code> contains a <code>NaN</code> value, or <code>dirX</code>,
87       *             <code>dirY</code>, or <code>dirZ</code> is <code>NaN</code>
88       */
89      public OrientedPoint3d(final double[] xyz, final double dirX, final double dirY, final double dirZ)
90              throws NullPointerException, IllegalArgumentException
91      {
92          super(xyz, dirY, dirZ);
93          Throw.whenNaN(dirX, "dirX");
94          this.dirX = dirX;
95      }
96  
97      /**
98       * Create a new OrientedPoint3d from another point and specified orientation dirX,dirY,dirZ.
99       * @param point the point from which this OrientedPoint3d will be instantiated
100      * @param dirX the direction as rotation around the x-axis with the point as the center
101      * @param dirY the direction as rotation around the y-axis with the point as the center
102      * @param dirZ the direction as rotation around the z-axis with the point as the center
103      * @throws NullPointerException when <code>point</code> is <code>null</code>
104      * @throws ArithmeticException when <code>dirX</code>, <code>dirY</code>, or <code>dirZ</code> is <code>NaN</code>
105      */
106     public OrientedPoint3d(final Point3d point, final double dirX, final double dirY, final double dirZ)
107     {
108         this(point.x, point.y, point.z, dirX, dirY, dirZ);
109     }
110 
111     /**
112      * Verify that a double array is not null, has three elements.
113      * @param orientation the array to check
114      * @return the first element of the argument
115      * @throws NullPointerException when <code>orientation</code> is <code>null</code>
116      * @throws IllegalArgumentException when the length of the <code>orientation</code> array is not 3
117      */
118     private static double checkOrientationVector(final double[] orientation)
119     {
120         Throw.when(orientation.length != 3, IllegalArgumentException.class, "length of orientation array must be 3");
121         return orientation[0];
122     }
123 
124     /**
125      * Create a new OrientedPoint3d with x, y, and z coordinates and orientation specified using a double array of three
126      * elements (containing dirX,dirY,dirZ in that order).
127      * @param x the x coordinate
128      * @param y the y coordinate
129      * @param z the z coordinate
130      * @param orientation the three orientation values as rotations around the x,y,z-axes in a double array containing
131      *            dirX,dirY,dirZ in that order
132      * @throws NullPointerException when <code>rotation</code> is <code>null</code>
133      * @throws IllegalArgumentException when the length of the <code>direction</code> array is not 3
134      */
135     public OrientedPoint3d(final double x, final double y, final double z, final double[] orientation)
136     {
137         this(x, y, z, checkOrientationVector(orientation), orientation[1], orientation[2]);
138     }
139 
140     /**
141      * Create a new OrientedPoint3d with x, y, and z coordinates packed in a double array and orientation specified using a
142      * double array of three elements (containing dirX,dirY,dirZ in that order).
143      * @param xyz the x, y and z coordinates in that order
144      * @param orientation the three orientation values as rotations around the x,y,z-axes in a double array containing
145      *            dirX,dirY,dirZ in that order
146      * @throws NullPointerException when <code>xyx</code> or <code>direction</code> is <code>null</code>
147      * @throws IllegalArgumentException when the length of the <code>xyx</code> array or the length of the
148      *             <code>orientation</code> array is not 3
149      */
150     public OrientedPoint3d(final double[] xyz, final double[] orientation)
151     {
152         this(xyz, checkOrientationVector(orientation), orientation[1], orientation[2]);
153     }
154 
155     @Override
156     public OrientedPoint3d translate(final double dX, final double dY)
157     {
158         Throw.whenNaN(dX, "dX");
159         Throw.whenNaN(dY, "dY");
160         return new OrientedPoint3d(this.x + dX, this.y + dY, this.z, this.dirX, this.dirY, this.dirZ);
161     }
162 
163     @Override
164     public OrientedPoint3d translate(final double dX, final double dY, final double dZ)
165     {
166         Throw.whenNaN(dX, "dX");
167         Throw.whenNaN(dY, "dY");
168         Throw.whenNaN(dZ, "dZ");
169         return new OrientedPoint3d(this.x + dX, this.y + dY, this.z + dZ, this.dirX, this.dirY, this.dirZ);
170     }
171 
172     @Override
173     public OrientedPoint3d scale(final double factor)
174     {
175         return new OrientedPoint3d(this.x * factor, this.y * factor, this.z * factor, this.dirX, this.dirY, this.dirZ);
176     }
177 
178     @Override
179     public OrientedPoint3d neg()
180     {
181         return new OrientedPoint3d(-this.x, -this.y, -this.z, AngleUtil.normalizeAroundZero(this.dirX + Math.PI),
182                 AngleUtil.normalizeAroundZero(this.dirY + Math.PI), AngleUtil.normalizeAroundZero(this.dirZ + Math.PI));
183     }
184 
185     @Override
186     public OrientedPoint3d abs()
187     {
188         return new OrientedPoint3d(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z), this.dirX, this.dirY, this.dirZ);
189     }
190 
191     @Override
192     public OrientedPoint3d normalize() throws IllegalArgumentException
193     {
194         double length = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
195         Throw.when(length == 0.0, IllegalArgumentException.class, "cannot normalize (0.0, 0.0, 0.0)");
196         return new OrientedPoint3d(this.x / length, this.y / length, this.z / length, this.dirX, this.dirY, this.dirZ);
197     }
198 
199     /**
200      * Interpolate towards another OrientedPoint3d with a fraction. It is allowed for fraction to be less than zero or larger
201      * than 1. In that case the interpolation turns into an extrapolation. DirX, dirY and dirZ are interpolated/extrapolated
202      * using the interpolateShortest method.
203      * @param otherPoint the other point
204      * @param fraction the factor for interpolation towards the other point. When &lt;code&gt;fraction&lt;/code&gt; is between 0
205      *            and 1, it is an interpolation, otherwise an extrapolation. If <code>fraction</code> is 0; <code>this</code>
206      *            Point is returned; if <code>fraction</code> is 1, the <code>otherPoint</code> is returned
207      * @return a new <code>OrientedPoint3d</code> at the requested <code>fraction</code>
208      * @throws NullPointerException when <code>otherPoint</code> is <code>null</code>
209      * @throws ArithmeticException when <code>fraction</code> is <code>NaN</code>
210      */
211     public OrientedPoint3d interpolate(final OrientedPoint3d otherPoint, final double fraction)
212     {
213         Throw.whenNull(otherPoint, "otherPoint");
214         Throw.whenNaN(fraction, "fraction");
215         if (0.0 == fraction)
216         {
217             return this;
218         }
219         if (1.0 == fraction)
220         {
221             return otherPoint;
222         }
223         return new OrientedPoint3d((1.0 - fraction) * this.x + fraction * otherPoint.x,
224                 (1.0 - fraction) * this.y + fraction * otherPoint.y, (1.0 - fraction) * this.z + fraction * otherPoint.z,
225                 AngleUtil.interpolateShortest(this.dirX, otherPoint.dirX, fraction),
226                 AngleUtil.interpolateShortest(this.dirY, otherPoint.dirY, fraction),
227                 AngleUtil.interpolateShortest(this.dirZ, otherPoint.dirZ, fraction));
228     }
229 
230     /**
231      * Return a new OrientedPoint3d with an in-place rotation around the z-axis by the provided rotateZ. The resulting rotation
232      * will be normalized between -&pi; and &pi;.
233      * @param rotateZ the rotation around the z-axis
234      * @return a new point with the same coordinates, <code>dirX</code> and <code>dirY</code> and modified <code>dirZ</code>
235      * @throws ArithmeticException when <code>rotateZ</code> is <code>NaN</code>
236      */
237     @Override
238     public OrientedPoint3d rotate(final double rotateZ)
239     {
240         Throw.whenNaN(rotateZ, "rotZ");
241         return new OrientedPoint3d(this.x, this.y, this.z, this.dirX, this.dirY,
242                 AngleUtil.normalizeAroundZero(this.dirZ + rotateZ));
243     }
244 
245     /**
246      * Return a new OrientedPoint3d point with an in-place rotation by the provided rotateX, rotateY, and rotateZ. The resulting
247      * rotations will be normalized between -&pi; and &pi;.
248      * @param rotateX the rotation around the x-axis
249      * @param rotateY the rotation around the y-axis
250      * @param rotateZ the rotation around the z-axis
251      * @return a new point with the same coordinates and applied rotations
252      * @throws ArithmeticException when any of the rotations is <code>NaN</code>
253      */
254     public OrientedPoint3d rotate(final double rotateX, final double rotateY, final double rotateZ)
255     {
256         Throw.whenNaN(rotateX, "rotateX");
257         Throw.whenNaN(rotateY, "rotateY");
258         Throw.whenNaN(rotateZ, "rotateZ");
259         return new OrientedPoint3d(this.x, this.y, this.z, AngleUtil.normalizeAroundZero(this.dirX + rotateX),
260                 AngleUtil.normalizeAroundZero(this.dirY + rotateY), AngleUtil.normalizeAroundZero(this.dirZ + rotateZ));
261     }
262 
263     @Override
264     public double getDirX()
265     {
266         return this.dirX;
267     }
268 
269     @Override
270     public double getDirY()
271     {
272         return this.dirY;
273     }
274 
275     @Override
276     public double getDirZ()
277     {
278         return this.dirZ;
279     }
280 
281     @Override
282     public Iterator<Point3d> iterator()
283     {
284         return Arrays.stream(new Point3d[] {this}).iterator();
285     }
286 
287     @Override
288     public String toString()
289     {
290         return toString("%f", false);
291     }
292 
293     @Override
294     public String toString(final String doubleFormat, final boolean doNotIncludeClassName)
295     {
296         String format = String.format("%1$s[x=%2$s, y=%2$s, z=%2$s, rotX=%2$s, rotY=%2$s, rotZ=%2$s]",
297                 doNotIncludeClassName ? "" : "OrientedPoint3d ", doubleFormat);
298         return String.format(Locale.US, format, this.x, this.y, this.z, this.dirX, this.dirY, this.dirZ);
299     }
300 
301     /**
302      * Compare this Directed with another Directed with specified tolerances in the coordinates and the angles.
303      * @param other the Directed to compare to
304      * @param epsilonCoordinate the upper bound of difference for one of the coordinates; use Double.POSITIVE_INFINITY if you do
305      *            not want to check the coordinates
306      * @param epsilonRotation the upper bound of difference for the rotation(s); use Double.POSITIVE_INFINITY if you do not want
307      *            to check the angles
308      * @return boolean;<code>true</code> if <code>x</code>, <code>y</code>, and possibly <code>z</code> are less than
309      *         <code>epsilonCoordinate</code> apart, and <code>rotZ</code> and possibly <code>rotX</code>, and possibly
310      *         <code>rotY</code>are less than <code>epsilonDirection</code> apart, otherwise <code>false</code>
311      * @throws NullPointerException when <code>other</code> is <code>null</code>
312      * @throws ArithmeticException when <code>epsilonCoordinate</code> or <code>epsilonDirection</code> is <code>NaN</code>
313      * @throws IllegalArgumentException <code>epsilonCoordinate</code> or <code>epsilonDirection</code> is <code>negative</code>
314      */
315     public boolean epsilonEquals(final OrientedPoint3d other, final double epsilonCoordinate, final double epsilonRotation)
316             throws NullPointerException, IllegalArgumentException
317     {
318         Throw.whenNull(other, "other");
319         if (Math.abs(this.x - other.x) > epsilonCoordinate)
320         {
321             return false;
322         }
323         if (Math.abs(this.y - other.y) > epsilonCoordinate)
324         {
325             return false;
326         }
327         if (Math.abs(this.z - other.z) > epsilonCoordinate)
328         {
329             return false;
330         }
331         if (Math.abs(AngleUtil.normalizeAroundZero(this.dirX - other.dirX)) > epsilonRotation)
332         {
333             return false;
334         }
335         if (Math.abs(AngleUtil.normalizeAroundZero(this.dirY - other.dirY)) > epsilonRotation)
336         {
337             return false;
338         }
339         if (Math.abs(AngleUtil.normalizeAroundZero(this.dirZ - other.dirZ)) > epsilonRotation)
340         {
341             return false;
342         }
343         return true;
344     }
345 
346     @Override
347     public int hashCode()
348     {
349         final int prime = 31;
350         int result = super.hashCode();
351         long temp;
352         temp = Double.doubleToLongBits(this.dirX);
353         result = prime * result + (int) (temp ^ (temp >>> 32));
354         temp = Double.doubleToLongBits(this.dirY);
355         result = prime * result + (int) (temp ^ (temp >>> 32));
356         temp = Double.doubleToLongBits(this.dirZ);
357         result = prime * result + (int) (temp ^ (temp >>> 32));
358         return result;
359     }
360 
361     @Override
362     @SuppressWarnings("checkstyle:needbraces")
363     public boolean equals(final Object obj)
364     {
365         if (this == obj)
366             return true;
367         if (!super.equals(obj))
368             return false;
369         OrientedPoint3d other = (OrientedPoint3d) obj;
370         if (Double.doubleToLongBits(this.dirX) != Double.doubleToLongBits(other.dirX))
371             return false;
372         if (Double.doubleToLongBits(this.dirY) != Double.doubleToLongBits(other.dirY))
373             return false;
374         if (Double.doubleToLongBits(this.dirZ) != Double.doubleToLongBits(other.dirZ))
375             return false;
376         return true;
377     }
378 
379 }