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
13
14
15
16
17
18
19
20
21
22
23
24
25 public class OrientedPoint3d extends DirectedPoint3d implements Oriented3d
26 {
27
28 private static final long serialVersionUID = 20200828L;
29
30
31 @SuppressWarnings("checkstyle:visibilitymodifier")
32 public final double dirX;
33
34
35
36
37
38
39
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
48
49
50
51
52
53
54
55
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
67
68
69
70
71
72 public OrientedPoint3d(final double[] xyz)
73 {
74 super(xyz, 0, 0);
75 this.dirX = 0.0;
76 }
77
78
79
80
81
82
83
84
85
86
87
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
99
100
101
102
103
104
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
113
114
115
116
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
126
127
128
129
130
131
132
133
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
142
143
144
145
146
147
148
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
201
202
203
204
205
206
207
208
209
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
232
233
234
235
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
247
248
249
250
251
252
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
303
304
305
306
307
308
309
310
311
312
313
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 }