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