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 }