1 package org.djutils.draw.point;
2
3 import java.util.Arrays;
4 import java.util.Iterator;
5 import java.util.Locale;
6 import java.util.Objects;
7
8 import org.djutils.draw.Directed;
9 import org.djutils.exceptions.Throw;
10 import org.djutils.math.AngleUtil;
11
12 /**
13 * A DirectedPoint2d is a Point2d that additionally carries a direction in 2d-space (dirZ). This is <b>not</b> the direction
14 * that the point is when viewed from the origin (0,0).
15 * <p>
16 * Copyright (c) 2023-2025 Delft University of Technology, Jaffalaan 5, 2628 BX Delft, the Netherlands. All rights reserved. See
17 * for project information <a href="https://djutils.org" target="_blank"> https://djutils.org</a>. The DJUTILS project is
18 * distributed under a three-clause BSD-style license, which can be found at
19 * <a href="https://djutils.org/docs/license.html" target="_blank"> https://djutils.org/docs/license.html</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 DirectedPoint2d extends Point2d implements Directed
26 {
27 /** */
28 private static final long serialVersionUID = 20200828L;
29
30 /** The counter-clockwise rotation around the point in radians. */
31 @SuppressWarnings("checkstyle:visibilitymodifier")
32 public final double dirZ;
33
34 /**
35 * Construct a new DirectedPoint2d with an x and y coordinate and a direction.
36 * @param x the x coordinate
37 * @param y the y coordinate
38 * @param dirZ the counter-clockwise rotation around the point in radians
39 * @throws IllegalArgumentException when any coordinate or <code>dirZ</code> is <code>NaN</code>
40 */
41 public DirectedPoint2d(final double x, final double y, final double dirZ)
42 {
43 super(x, y);
44 Throw.whenNaN(dirZ, "dirZ");
45 this.dirZ = dirZ;
46 }
47
48 /**
49 * Construct a new DirectedPoint2d from an x and y coordinates in a double[] and a direction.
50 * @param xy the <code>x</code> and <code>y</code> coordinates in that order
51 * @param dirZ the counter-clockwise rotation around the point in radians
52 * @throws NullPointerException when <code>xy</code> is <code>null</code>
53 * @throws ArithmeticException when any value in <code>xy</code> is <code>NaN</code> or <code>rotZ</code> is
54 * <code>NaN</code>
55 * @throws IllegalArgumentException when the length of <code>xy</code> is not 2
56 */
57 public DirectedPoint2d(final double[] xy, final double dirZ)
58 {
59 super(xy);
60 Throw.whenNaN(dirZ, "dirZ");
61 this.dirZ = dirZ;
62 }
63
64 /**
65 * Construct a new DirectedPoint2d from an AWT Point2D and a direction.
66 * @param point java.awt.geom.Point2D
67 * @param dirZ the counter-clockwise rotation around the point in radians
68 * @throws NullPointerException when <code>point</code> is <code>null</code>
69 * @throws ArithmeticException when <code>rotZ</code> is <code>NaN</code>
70 */
71 public DirectedPoint2d(final java.awt.geom.Point2D point, final double dirZ)
72 {
73 super(point);
74 Throw.whenNaN(dirZ, "dirZ");
75 this.dirZ = dirZ;
76 }
77
78 /**
79 * Construct a new DirectedPoint2d from a Point2d and a direction.
80 * @param point a point (with or without orientation)
81 * @param dirZ the counter-clockwise rotation around the point in radians
82 * @throws NullPointerException when <code>point</code> is <code>null</code>
83 * @throws ArithmeticException when <code>rotZ</code> is <code>NaN</code>
84 */
85 public DirectedPoint2d(final Point2d point, final double dirZ)
86 {
87 this(point.x, point.y, dirZ);
88 }
89
90 /**
91 * Construct a new DirectedPoint2d from two coordinates and the coordinates of a point that the direction goes through.
92 * @param x the x coordinate of the of the new DirectedPoint
93 * @param y the y coordinate of the of the new DirectedPoint
94 * @param throughX the x-coordinate of a point that the direction goes through
95 * @param throughY the y-coordinate of a point that the direction goes through
96 * @throws ArithmeticException when <code>throughX</code>, or <code>throughY</code> is <code>null</code>
97 * @throws IllegalArgumentException when <code>throughX == x</code> and <code>throughY == y</code>
98 */
99 public DirectedPoint2d(final double x, final double y, final double throughX, final double throughY)
100 {
101 this(x, y, buildDirection(throughX - x, throughY - y));
102 }
103
104 /**
105 * Build the direction.
106 * @param dX x difference
107 * @param dY y difference
108 * @return the computed value of dirZ
109 * @throws IllegalArgumentException when <code>dX == 0.0</code> and <code>dY == 0.0</code>
110 */
111 private static double buildDirection(final double dX, final double dY)
112 {
113 Throw.when(0 == dX && 0 == dY, IllegalArgumentException.class, "Through point may not be equal to point");
114 return Math.atan2(dY, dX);
115 }
116
117 /**
118 * Construct a new DirectedPoint2d from a Point2d and a point that the direction goes through.
119 * @param point the point
120 * @param throughPoint the point that the direction goes through
121 * @throws NullPointerException when <code>point</code> is <code>null</code>, or <code>throughPoint</code> ==
122 * <code>null</code>
123 * @throws IllegalArgumentException when <code>throughX == point.x</code> and <code>throughY ==
124 * point.y</code>
125 */
126 public DirectedPoint2d(final Point2d point, final Point2d throughPoint)
127 {
128 this(Throw.whenNull(point, "point").x, point.y, Throw.whenNull(throughPoint, "throughPoint").x, throughPoint.y);
129 }
130
131 /**
132 * Construct a new DirectedPoint2d from a Point2d and the coordinates of a point that the direction goes through.
133 * @param point the point
134 * @param throughX the x coordinate of a point that the direction goes through
135 * @param throughY the y coordinate of a point that the direction goes through
136 * @throws NullPointerException when <code>point</code> is <code>null</code>
137 * @throws ArithmeticException when <code>throughX</code>, or <code>throughY</code> is <code>NaN</code>
138 * @throws IllegalArgumentException when <code>throughX == point.x</code> and <code>throughY ==
139 * point.y</code>
140 */
141 public DirectedPoint2d(final Point2d point, final double throughX, final double throughY)
142 {
143 this(Throw.whenNull(point, "point").x, point.y, throughX, throughY);
144 }
145
146 @Override
147 public DirectedPoint2d translate(final double dX, final double dY)
148 {
149 return new DirectedPoint2d(this.x + dX, this.y + dY, this.dirZ);
150 }
151
152 @Override
153 public DirectedPoint3d translate(final double dX, final double dY, final double z)
154 throws ArithmeticException, IllegalArgumentException
155 {
156 return new DirectedPoint3d(this.x + dX, this.y + dY, z, 0, this.dirZ);
157 }
158
159 @Override
160 public DirectedPoint2d scale(final double factor) throws IllegalArgumentException
161 {
162 Throw.whenNaN(factor, "factor");
163 return new DirectedPoint2d(this.x * factor, this.y * factor, this.dirZ);
164 }
165
166 @Override
167 public DirectedPoint2d neg()
168 {
169 return new DirectedPoint2d(-this.x, -this.y, AngleUtil.normalizeAroundZero(this.dirZ + Math.PI));
170 }
171
172 @Override
173 public DirectedPoint2d abs()
174 {
175 return new DirectedPoint2d(Math.abs(this.x), Math.abs(this.y), this.dirZ);
176 }
177
178 @Override
179 public DirectedPoint2d normalize() throws IllegalArgumentException
180 {
181 double length = Math.sqrt(this.x * this.x + this.y * this.y);
182 Throw.when(length == 0.0, IllegalArgumentException.class, "cannot normalize (0.0, 0.0)");
183 return this.scale(1.0 / length);
184 }
185
186 /**
187 * Interpolate towards another DirectedPoint2d with a fraction. It is allowed for fraction to be less than zero or larger
188 * than 1. In that case the interpolation turns into an extrapolation. DirZ is interpolated using the
189 * AngleUtil.interpolateShortest method.
190 * @param otherPoint the other point
191 * @param fraction the factor for interpolation towards the other point. When <code>fraction</code> is between 0
192 * and 1, it is an interpolation, otherwise an extrapolation. If <code>fraction</code> is 0; <code>this</code>
193 * Point is returned; if <code>fraction</code> is 1, the <code>otherPoint</code> is returned
194 * @return a new OrientedPoint2d at the requested fraction
195 * @throws NullPointerException when <code>otherPoint</code> is <code>null</code>
196 * @throws ArithmeticException when <code>fraction</code> is <code>NaN</code>
197 */
198 public DirectedPoint2d interpolate(final DirectedPoint2d otherPoint, final double fraction)
199 {
200 Throw.whenNull(otherPoint, "otherPoint");
201 Throw.whenNaN(fraction, "fraction");
202 if (0.0 == fraction)
203 {
204 return this;
205 }
206 if (1.0 == fraction)
207 {
208 return otherPoint;
209 }
210 return new DirectedPoint2d((1.0 - fraction) * this.x + fraction * otherPoint.x,
211 (1.0 - fraction) * this.y + fraction * otherPoint.y,
212 AngleUtil.interpolateShortest(this.dirZ, otherPoint.dirZ, fraction));
213 }
214
215 /**
216 * Return a new DirectedPoint2d with an in-place rotation around the z-axis by the provided rotateZ. The resulting rotation
217 * is normalized between -π and π.
218 * @param rotateZ the rotation around the z-axis
219 * @return a new point with the same coordinates and applied rotation
220 * @throws ArithmeticException when <code>rotateZ</code> is <code>NaN</code>
221 */
222 public DirectedPoint2d rotate(final double rotateZ)
223 {
224 Throw.whenNaN(rotateZ, "rotateZ");
225 return new DirectedPoint2d(this.x, this.y, AngleUtil.normalizeAroundZero(this.dirZ + rotateZ));
226 }
227
228 @Override
229 public double getDirZ()
230 {
231 return this.dirZ;
232 }
233
234 @Override
235 public Iterator<Point2d> iterator()
236 {
237 return Arrays.stream(new Point2d[] {this}).iterator();
238 }
239
240 @Override
241 public String toString()
242 {
243 return toString("%f", false);
244 }
245
246 @Override
247 public String toString(final String doubleFormat, final boolean doNotIncludeClassName)
248 {
249 String format =
250 String.format("%1$s[x=%2$s, y=%2$s, dirZ=%2$s]", doNotIncludeClassName ? "" : "DirectedPoint2d ", doubleFormat);
251 return String.format(Locale.US, format, this.x, this.y, this.dirZ);
252 }
253
254 /**
255 * Compare this Directed with another Directed with specified tolerances in the coordinates and the angles.
256 * @param other the Directed to compare to
257 * @param epsilonCoordinate the upper bound of difference for one of the coordinates; use Double.POSITIVE_INFINITY if you do
258 * not want to check the coordinates
259 * @param epsilonDirection the upper bound of difference for the direction(s); use Double.POSITIVE_INFINITY if you do not
260 * want to check the angles
261 * @return boolean;<code>true</code> if <code>x</code> and <code>y</code> are less than <code>epsilonCoordinate</code>
262 * apart, and <code>rotZ</code> is less than <code>epsilonDirection</code> apart, otherwise <code>false</code>
263 * @throws NullPointerException when <code>other</code> is <code>null</code>
264 * @throws ArithmeticException when <code>epsilonCoordinate</code> or <code>epsilonDirection</code> is <code>NaN</code>
265 * @throws IllegalArgumentException <code>epsilonCoordinate</code> or <code>epsilonDirection</code> is <code>negative</code>
266 */
267 public boolean epsilonEquals(final DirectedPoint2d other, final double epsilonCoordinate, final double epsilonDirection)
268 throws NullPointerException, IllegalArgumentException
269 {
270 Throw.whenNull(other, "other");
271 Throw.whenNaN(epsilonCoordinate, "epsilonCoordinate");
272 Throw.whenNaN(epsilonDirection, "epsilonDirection");
273 Throw.when(epsilonCoordinate < 0 || epsilonDirection < 0, IllegalArgumentException.class,
274 "epsilonCoordinate and epsilonRotation may not be negative");
275 if (Math.abs(this.x - other.x) > epsilonCoordinate)
276 {
277 return false;
278 }
279 if (Math.abs(this.y - other.y) > epsilonCoordinate)
280 {
281 return false;
282 }
283 if (Math.abs(AngleUtil.normalizeAroundZero(this.dirZ - other.dirZ)) > epsilonDirection)
284 {
285 return false;
286 }
287 return true;
288 }
289
290 @Override
291 public int hashCode()
292 {
293 final int prime = 31;
294 int result = super.hashCode();
295 result = prime * result + Objects.hash(this.dirZ);
296 return result;
297 }
298
299 @Override
300 @SuppressWarnings("checkstyle:needbraces")
301 public boolean equals(final Object obj)
302 {
303 if (this == obj)
304 return true;
305 if (!super.equals(obj))
306 return false;
307 if (getClass() != obj.getClass())
308 return false;
309 DirectedPoint2d other = (DirectedPoint2d) obj;
310 return Double.doubleToLongBits(this.dirZ) == Double.doubleToLongBits(other.dirZ);
311 }
312
313 }