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.base.AngleUtil;
8 import org.djutils.draw.DrawRuntimeException;
9 import org.djutils.draw.Oriented3d;
10 import org.djutils.exceptions.Throw;
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25 public class OrientedPoint3d extends Point3d implements Oriented3d<OrientedPoint3d>
26 {
27
28 private static final long serialVersionUID = 20200828L;
29
30
31 @SuppressWarnings("checkstyle:visibilitymodifier")
32 public final double dirX;
33
34
35 @SuppressWarnings("checkstyle:visibilitymodifier")
36 public final double dirY;
37
38
39 @SuppressWarnings("checkstyle:visibilitymodifier")
40 public final double dirZ;
41
42
43
44
45
46
47
48
49 public OrientedPoint3d(final double x, final double y, final double z) throws IllegalArgumentException
50 {
51 super(x, y, z);
52 this.dirX = 0.0;
53 this.dirY = 0.0;
54 this.dirZ = 0.0;
55 }
56
57
58
59
60
61
62
63
64
65
66
67 public OrientedPoint3d(final double x, final double y, final double z, final double dirX, final double dirY,
68 final double dirZ) throws IllegalArgumentException
69 {
70 super(x, y, z);
71 Throw.when(Double.isNaN(dirX) || Double.isNaN(dirY) || Double.isNaN(dirZ), IllegalArgumentException.class,
72 "Rotation must be a number (not NaN)");
73 this.dirX = dirX;
74 this.dirY = dirY;
75 this.dirZ = dirZ;
76 }
77
78
79
80
81
82
83
84 public OrientedPoint3d(final double[] xyz) throws NullPointerException, IllegalArgumentException
85 {
86 super(xyz);
87 this.dirX = 0.0;
88 this.dirY = 0.0;
89 this.dirZ = 0.0;
90 }
91
92
93
94
95
96
97
98
99
100
101
102 public OrientedPoint3d(final double[] xyz, final double dirX, final double dirY, final double dirZ)
103 throws NullPointerException, IllegalArgumentException
104 {
105 super(xyz);
106 Throw.when(Double.isNaN(dirX) || Double.isNaN(dirY) || Double.isNaN(dirZ), IllegalArgumentException.class,
107 "Direction must be a number (not NaN)");
108 this.dirX = dirX;
109 this.dirY = dirY;
110 this.dirZ = dirZ;
111 }
112
113
114
115
116
117
118
119
120
121 public OrientedPoint3d(final Point3d point, final double dirX, final double dirY, final double dirZ)
122 throws IllegalArgumentException
123 {
124 super(point.x, point.y, point.z);
125 Throw.when(Double.isNaN(dirX) || Double.isNaN(dirY) || Double.isNaN(dirZ), IllegalArgumentException.class,
126 "Direction must be a number (not NaN)");
127 this.dirX = dirX;
128 this.dirY = dirY;
129 this.dirZ = dirZ;
130 }
131
132
133
134
135
136
137
138
139
140
141
142
143 public OrientedPoint3d(final double x, final double y, final double z, final double[] orientation)
144 throws NullPointerException, IllegalArgumentException
145 {
146 super(x, y, z);
147 Throw.whenNull(orientation, "direction array cannot be null");
148 Throw.when(orientation.length != 3, IllegalArgumentException.class, "length of direction array must be 3");
149 this.dirX = orientation[0];
150 this.dirY = orientation[1];
151 this.dirZ = orientation[2];
152 }
153
154
155
156
157
158
159
160
161
162
163 public OrientedPoint3d(final double[] xyz, final double[] orientation) throws NullPointerException, IllegalArgumentException
164 {
165 super(xyz);
166 Throw.whenNull(orientation, "direction cannot be null");
167 Throw.when(orientation.length != 3, IllegalArgumentException.class, "length of direction array must be 3");
168 this.dirX = orientation[0];
169 this.dirY = orientation[1];
170 this.dirZ = orientation[2];
171 }
172
173
174 @Override
175 public OrientedPoint3d translate(final double dx, final double dy) throws IllegalArgumentException
176 {
177 Throw.when(Double.isNaN(dx) || Double.isNaN(dy), IllegalArgumentException.class, "translation may not be NaN");
178 return new OrientedPoint3d(getX() + dx, getY() + dy, getZ(), getDirX(), getDirY(), getDirZ());
179 }
180
181
182 @Override
183 public OrientedPoint3d translate(final double dx, final double dy, final double dz) throws IllegalArgumentException
184 {
185 Throw.when(Double.isNaN(dx) || Double.isNaN(dy) || Double.isNaN(dz), IllegalArgumentException.class,
186 "Translation may not be NaN");
187 return new OrientedPoint3d(this.x + dx, this.y + dy, this.z + dz, this.dirX, this.dirY, this.dirZ);
188 }
189
190
191 @Override
192 public OrientedPoint3d scale(final double factor) throws IllegalArgumentException
193 {
194 return new OrientedPoint3d(this.x * factor, this.y * factor, this.z * factor, this.dirX, this.dirY, this.dirZ);
195 }
196
197
198 @Override
199 public OrientedPoint3d neg()
200 {
201 return new OrientedPoint3d(-this.x, -this.y, -this.z, AngleUtil.normalizeAroundZero(this.dirX + Math.PI),
202 AngleUtil.normalizeAroundZero(this.dirY + Math.PI), AngleUtil.normalizeAroundZero(this.dirZ + Math.PI));
203 }
204
205
206 @Override
207 public OrientedPoint3d abs()
208 {
209 return new OrientedPoint3d(Math.abs(this.x), Math.abs(this.y), Math.abs(this.z), this.dirX, this.dirY, this.dirZ);
210 }
211
212
213 @Override
214 public OrientedPoint3d normalize() throws DrawRuntimeException
215 {
216 double length = Math.sqrt(getX() * getX() + getY() * getY() + getZ() * getZ());
217 Throw.when(length == 0.0, DrawRuntimeException.class, "cannot normalize (0.0, 0.0, 0.0)");
218 return new OrientedPoint3d(this.x / length, this.y / length, this.z / length, this.dirX, this.dirY, this.dirZ);
219 }
220
221
222
223
224
225
226
227
228
229
230
231
232
233 public OrientedPoint3d interpolate(final OrientedPoint3d otherPoint, final double fraction)
234 throws NullPointerException, IllegalArgumentException
235 {
236 Throw.whenNull(otherPoint, "point cannot be null");
237 Throw.when(Double.isNaN(fraction), IllegalArgumentException.class, "fraction must be a number (not NaN)");
238 return new OrientedPoint3d((1.0 - fraction) * getX() + fraction * otherPoint.x,
239 (1.0 - fraction) * getY() + fraction * otherPoint.y, (1.0 - fraction) * getZ() + fraction * otherPoint.z,
240 AngleUtil.interpolateShortest(getDirX(), otherPoint.getDirX(), fraction),
241 AngleUtil.interpolateShortest(getDirY(), otherPoint.getDirY(), fraction),
242 AngleUtil.interpolateShortest(getDirZ(), otherPoint.getDirZ(), fraction));
243 }
244
245
246
247
248
249
250
251
252 public OrientedPoint3d rotate(final double rotateZ) throws IllegalArgumentException
253 {
254 Throw.when(Double.isNaN(rotateZ), IllegalArgumentException.class, "deltaDirZ must be a number (not NaN)");
255 return new OrientedPoint3d(getX(), getY(), getZ(), getDirX(), getDirY(),
256 AngleUtil.normalizeAroundZero(getDirZ() + rotateZ));
257 }
258
259
260
261
262
263
264
265
266
267
268 public OrientedPoint3d rotate(final double rotateX, final double rotateY, final double rotateZ)
269 throws IllegalArgumentException
270 {
271 Throw.when(Double.isNaN(rotateX) || Double.isNaN(rotateY) || Double.isNaN(rotateZ), IllegalArgumentException.class,
272 "Rotation must be a number (not NaN)");
273 return new OrientedPoint3d(getX(), getY(), getZ(), AngleUtil.normalizeAroundZero(getDirX() + rotateX),
274 AngleUtil.normalizeAroundZero(getDirY() + rotateY), AngleUtil.normalizeAroundZero(getDirZ() + rotateZ));
275 }
276
277
278 @Override
279 public double getDirX()
280 {
281 return this.dirX;
282 }
283
284
285 @Override
286 public double getDirY()
287 {
288 return this.dirY;
289 }
290
291
292 @Override
293 public double getDirZ()
294 {
295 return this.dirZ;
296 }
297
298
299 @Override
300 public Iterator<OrientedPoint3d> getPoints()
301 {
302 return Arrays.stream(new OrientedPoint3d[] { this }).iterator();
303 }
304
305
306 @Override
307 public String toString()
308 {
309 return toString("%f", false);
310 }
311
312
313 @Override
314 public String toString(final String doubleFormat, final boolean doNotIncludeClassName)
315 {
316 String format = String.format("%1$s[x=%2$s, y=%2$s, z=%2$s, rotX=%2$s, rotY=%2$s, rotZ=%2$s]",
317 doNotIncludeClassName ? "" : "OrientedPoint3d ", doubleFormat);
318 return String.format(Locale.US, format, this.x, this.y, this.z, this.dirX, this.dirY, this.dirZ);
319 }
320
321
322 @Override
323 public boolean epsilonEquals(final OrientedPoint3d other, final double epsilonCoordinate, final double epsilonRotation)
324 throws NullPointerException, IllegalArgumentException
325 {
326 Throw.whenNull(other, "other point cannot be null");
327 if (Math.abs(this.x - other.x) > epsilonCoordinate)
328 {
329 return false;
330 }
331 if (Math.abs(this.y - other.y) > epsilonCoordinate)
332 {
333 return false;
334 }
335 if (Math.abs(this.z - other.z) > epsilonCoordinate)
336 {
337 return false;
338 }
339 if (Math.abs(AngleUtil.normalizeAroundZero(this.dirX - other.dirX)) > epsilonRotation)
340 {
341 return false;
342 }
343 if (Math.abs(AngleUtil.normalizeAroundZero(this.dirY - other.dirY)) > epsilonRotation)
344 {
345 return false;
346 }
347 if (Math.abs(AngleUtil.normalizeAroundZero(this.dirZ - other.dirZ)) > epsilonRotation)
348 {
349 return false;
350 }
351 return true;
352 }
353
354
355 @Override
356 public int hashCode()
357 {
358 final int prime = 31;
359 int result = super.hashCode();
360 long temp;
361 temp = Double.doubleToLongBits(this.dirX);
362 result = prime * result + (int) (temp ^ (temp >>> 32));
363 temp = Double.doubleToLongBits(this.dirY);
364 result = prime * result + (int) (temp ^ (temp >>> 32));
365 temp = Double.doubleToLongBits(this.dirZ);
366 result = prime * result + (int) (temp ^ (temp >>> 32));
367 return result;
368 }
369
370
371 @Override
372 @SuppressWarnings("checkstyle:needbraces")
373 public boolean equals(final Object obj)
374 {
375 if (this == obj)
376 return true;
377 if (!super.equals(obj))
378 return false;
379 OrientedPoint3d other = (OrientedPoint3d) obj;
380 if (Double.doubleToLongBits(this.dirX) != Double.doubleToLongBits(other.dirX))
381 return false;
382 if (Double.doubleToLongBits(this.dirY) != Double.doubleToLongBits(other.dirY))
383 return false;
384 if (Double.doubleToLongBits(this.dirZ) != Double.doubleToLongBits(other.dirZ))
385 return false;
386 return true;
387 }
388
389 }