1 package org.djutils.draw;
2
3 import java.util.Arrays;
4 import java.util.Iterator;
5
6 import org.djutils.draw.bounds.Bounds3d;
7 import org.djutils.draw.point.Point3d;
8
9 /**
10 * Transform3d contains a MUTABLE transformation object that can transform points (x,y,z) based on e.g, rotation and
11 * translation. It uses an affine transform matrix that can be built up from different components (translation, rotation,
12 * scaling, reflection, shearing).
13 * <p>
14 * Copyright (c) 2020-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
15 * BSD-style license. See <a href="https://djutils.org/docs/current/djutils/licenses.html">DJUTILS License</a>.
16 * </p>
17 * @author <a href="https://www.tudelft.nl/averbraeck">Alexander Verbraeck</a>
18 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a>
19 */
20 public class Transform3d implements Cloneable
21 {
22 /** The 4x4 transformation matrix, initialized as the Identity matrix. */
23 private double[] mat = new double[] {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
24
25 /**
26 * Multiply a 4x4 matrix (stored as a 16-value array by row) with a 4-value vector.
27 * @param m double[]; the matrix
28 * @param v double[]; the vector
29 * @return double[4]; the result of m x v
30 */
31 protected static double[] mulMatVec(final double[] m, final double[] v)
32 {
33 double[] result = new double[4];
34 for (int i = 0; i < 4; i++)
35 {
36 result[i] = m[4 * i] * v[0] + m[4 * i + 1] * v[1] + m[4 * i + 2] * v[2] + m[4 * i + 3] * v[3];
37 }
38 return result;
39 }
40
41 /**
42 * Multiply a 4x4 matrix (stored as a 16-value array by row) with a 3-value vector and a 1 for the 4th value.
43 * @param m double[]; the matrix
44 * @param v double[]; the vector
45 * @return double[3]; the result of m x (v1, v2, v3, 1), with the last value left out
46 */
47 protected static double[] mulMatVec3(final double[] m, final double[] v)
48 {
49 double[] result = new double[3];
50 for (int i = 0; i < 3; i++)
51 {
52 result[i] = m[4 * i] * v[0] + m[4 * i + 1] * v[1] + m[4 * i + 2] * v[2] + m[4 * i + 3];
53 }
54 return result;
55 }
56
57 /**
58 * Multiply a 4x4 matrix (stored as a 16-value array by row) with another 4x4-matrix.
59 * @param m1 double[]; the first matrix
60 * @param m2 double[]; the second matrix
61 * @return double[16]; the result of m1 x m2
62 */
63 protected static double[] mulMatMat(final double[] m1, final double[] m2)
64 {
65 double[] result = new double[16];
66 for (int i = 0; i < 4; i++)
67 {
68 for (int j = 0; j < 4; j++)
69 {
70 result[4 * i + j] =
71 m1[4 * i] * m2[j] + m1[4 * i + 1] * m2[j + 4] + +m1[4 * i + 2] * m2[j + 8] + m1[4 * i + 3] * m2[j + 12];
72 }
73 }
74 return result;
75 }
76
77 /**
78 * Get a safe copy of the affine transformation matrix.
79 * @return double[]; a safe copy of the affine transformation matrix
80 */
81 public double[] getMat()
82 {
83 return this.mat.clone();
84 }
85
86 /**
87 * Transform coordinates by a vector (tx, ty, tz). Note that to carry out multiple operations, the steps have to be built in
88 * the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
89 * @param tx double; the translation value for the x-coordinates
90 * @param ty double; the translation value for the y-coordinates
91 * @param tz double; the translation value for the z-coordinates
92 * @return Transform3d; the new transformation matrix after applying this transform
93 */
94 public Transform3d translate(final double tx, final double ty, final double tz)
95 {
96 if (tx == 0.0 && ty == 0.0 && tz == 0.0)
97 {
98 return this;
99 }
100 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, tx, 0, 1, 0, ty, 0, 0, 1, tz, 0, 0, 0, 1});
101 return this;
102 }
103
104 /**
105 * Translate coordinates by a the x, y, and z values contained in a Point. Note that to carry out multiple operations, the
106 * steps have to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
107 * @param point Point3d; the point containing the x, y, and z translation values
108 * @return Transform3d; the new transformation matrix after applying this transform
109 */
110 public Transform3d translate(final Point3d point)
111 {
112 if (point.x == 0.0 && point.y == 0.0 && point.z == 0.0)
113 {
114 return this;
115 }
116 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, point.x, 0, 1, 0, point.y, 0, 0, 1, point.z, 0, 0, 0, 1});
117 return this;
118 }
119
120 /**
121 * Scale all coordinates with a factor for x, y, and z. A scale factor of 1 leaves the coordinate unchanged. Note that to
122 * carry out multiple operations, the steps have to be built in the OPPOSITE order since matrix multiplication operates from
123 * RIGHT to LEFT.
124 * @param sx double; the scale factor for the x-coordinates
125 * @param sy double; the scale factor for the y-coordinates
126 * @param sz double; the scale factor for the z-coordinates
127 * @return Transform3d; the new transformation matrix after applying this transform
128 */
129 public Transform3d scale(final double sx, final double sy, final double sz)
130 {
131 if (sx == 1.0 && sy == 1.0 && sz == 1.0)
132 {
133 return this;
134 }
135 this.mat = mulMatMat(this.mat, new double[] {sx, 0, 0, 0, 0, sy, 0, 0, 0, 0, sz, 0, 0, 0, 0, 1});
136 return this;
137 }
138
139 /**
140 * The Euler rotation around the x-axis with an angle in radians. Note that to carry out multiple operations, the steps have
141 * to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
142 * @param angle double; the angle to rotate the coordinates with with around the x-axis
143 * @return Transform3d; the new transformation matrix after applying this transform
144 */
145 public Transform3d rotX(final double angle)
146 {
147 if (angle == 0.0)
148 {
149 return this;
150 }
151 double c = Math.cos(angle);
152 double s = Math.sin(angle);
153 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, 0, 0, c, -s, 0, 0, s, c, 0, 0, 0, 0, 1});
154 return this;
155 }
156
157 /**
158 * The Euler rotation around the y-axis with an angle in radians. Note that to carry out multiple operations, the steps have
159 * to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
160 * @param angle double; the angle to rotate the coordinates with with around the y-axis
161 * @return Transform3d; the new transformation matrix after applying this transform
162 */
163 public Transform3d rotY(final double angle)
164 {
165 if (angle == 0.0)
166 {
167 return this;
168 }
169 double c = Math.cos(angle);
170 double s = Math.sin(angle);
171 this.mat = mulMatMat(this.mat, new double[] {c, 0, s, 0, 0, 1, 0, 0, -s, 0, c, 0, 0, 0, 0, 1});
172 return this;
173 }
174
175 /**
176 * The Euler rotation around the z-axis with an angle in radians. Note that to carry out multiple operations, the steps have
177 * to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
178 * @param angle double; the angle to rotate the coordinates with with around the z-axis
179 * @return Transform3d; the new transformation matrix after applying this transform
180 */
181 public Transform3d rotZ(final double angle)
182 {
183 if (angle == 0.0)
184 {
185 return this;
186 }
187 double c = Math.cos(angle);
188 double s = Math.sin(angle);
189 this.mat = mulMatMat(this.mat, new double[] {c, -s, 0, 0, s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1});
190 return this;
191 }
192
193 /**
194 * The xy-shear leaves the xy-coordinate plane for z=0 untouched. Coordinates on z=1 are translated by a vector (sx, sy, 0).
195 * Coordinates for points with other z-values are translated by a vector (z.sx, z.sy, 0), where z is the z-coordinate of the
196 * point. Note that to carry out multiple operations, the steps have to be built in the OPPOSITE order since matrix
197 * multiplication operates from RIGHT to LEFT.
198 * @param sx double; the shear factor in the x-direction for z=1
199 * @param sy double; the shear factor in the y-direction for z=1
200 * @return Transform3d; the new transformation matrix after applying this transform
201 */
202 public Transform3d shearXY(final double sx, final double sy)
203 {
204 if (sx == 0.0 && sy == 0.0)
205 {
206 return this;
207 }
208 this.mat = mulMatMat(this.mat, new double[] {1, 0, sx, 0, 0, 1, sy, 0, 0, 0, 1, 0, 0, 0, 0, 1});
209 return this;
210 }
211
212 /**
213 * The yz-shear leaves the yz-coordinate plain for x=0 untouched. Coordinates on x=1 are translated by a vector (0, sy, sz).
214 * Coordinates for points with other x-values are translated by a vector (0, x.sy, x.sz), where x is the x-coordinate of the
215 * point. Note that to carry out multiple operations, the steps have to be built in the OPPOSITE order since matrix
216 * multiplication operates from RIGHT to LEFT.
217 * @param sy double; the shear factor in the y-direction for x=1
218 * @param sz double; the shear factor in the z-direction for x=1
219 * @return Transform3d; the new transformation matrix after applying this transform
220 */
221 public Transform3d shearYZ(final double sy, final double sz)
222 {
223 if (sy == 0.0 && sz == 0.0)
224 {
225 return this;
226 }
227 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, 0, sy, 1, 0, 0, sz, 0, 1, 0, 0, 0, 0, 1});
228 return this;
229 }
230
231 /**
232 * The xz-shear leaves the xz-coordinate plain for y=0 untouched. Coordinates on y=1 are translated by a vector (sx, 0, sz).
233 * Coordinates for points with other y-values are translated by a vector (y.sx, 0, y.sz), where y is the y-coordinate of the
234 * point. Note that to carry out multiple operations, the steps have to be built in the OPPOSITE order since matrix
235 * multiplication operates from RIGHT to LEFT.
236 * @param sx double; the shear factor in the y-direction for y=1
237 * @param sz double; the shear factor in the z-direction for y=1
238 * @return Transform3d; the new transformation matrix after applying this transform
239 */
240 public Transform3d shearXZ(final double sx, final double sz)
241 {
242 if (sx == 0.0 && sz == 0.0)
243 {
244 return this;
245 }
246 this.mat = mulMatMat(this.mat, new double[] {1, sx, 0, 0, 0, 1, 0, 0, 0, sz, 1, 0, 0, 0, 0, 1});
247 return this;
248 }
249
250 /**
251 * The reflection of the x-coordinate, by mirroring it in the yz-plane (the plane with x=0). Note that to carry out multiple
252 * operations, the steps have to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
253 * @return Transform3d; the new transformation matrix after applying this transform
254 */
255 public Transform3d reflectX()
256 {
257 this.mat = mulMatMat(this.mat, new double[] {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1});
258 return this;
259 }
260
261 /**
262 * The reflection of the y-coordinate, by mirroring it in the xz-plane (the plane with y=0). Note that to carry out multiple
263 * operations, the steps have to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
264 * @return Transform3d; the new transformation matrix after applying this transform
265 */
266 public Transform3d reflectY()
267 {
268 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1});
269 return this;
270 }
271
272 /**
273 * The reflection of the z-coordinate, by mirroring it in the xy-plane (the plane with z=0). Note that to carry out multiple
274 * operations, the steps have to be built in the OPPOSITE order since matrix multiplication operates from RIGHT to LEFT.
275 * @return Transform3d; the new transformation matrix after applying this transform
276 */
277 public Transform3d reflectZ()
278 {
279 this.mat = mulMatMat(this.mat, new double[] {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1});
280 return this;
281 }
282
283 /**
284 * Apply the stored transform on the xyz-vector and return the transformed vector. For speed reasons, no checks on correct
285 * size of the vector is done.
286 * @param xyz double[]; double[3] the provided vector
287 * @return double[3]; the transformed vector
288 */
289 public double[] transform(final double[] xyz)
290 {
291 return mulMatVec3(this.mat, xyz);
292 }
293
294 /**
295 * Apply the stored transform on the provided point and return a point with the transformed coordinate.
296 * @param point Point3d; the point to be transformed
297 * @return Point3d; a point with the transformed coordinates
298 */
299 public Point3d transform(final Point3d point)
300 {
301 return new Point3d(mulMatVec3(this.mat, new double[] {point.x, point.y, point.z}));
302 }
303
304 /**
305 * Apply the stored transform on the provided point and return a point with the transformed coordinate.
306 * @param pointIterator Iterator<Point3d>; generates the points to be transformed
307 * @return Iterator<Point3d>; an iterator that will generator all transformed points
308 */
309 public Iterator<Point3d> transform(final Iterator<Point3d> pointIterator)
310 {
311 return new Iterator<Point3d>()
312 {
313
314 @Override
315 public boolean hasNext()
316 {
317 return pointIterator.hasNext();
318 }
319
320 @Override
321 public Point3d next()
322 {
323 return transform(pointIterator.next());
324 }
325 };
326 }
327
328 /**
329 * Apply the stored transform on the provided Bounds3d and return a new Bounds3d with the bounds of the transformed
330 * coordinates. All 8 corner points have to be transformed, since we do not know which of the 8 points will result in the
331 * lowest and highest x, y, and z coordinates.
332 * @param boundingBox Bounds3d; the bounds to be transformed
333 * @return Bounds3d; the new bounds based on the transformed coordinates
334 */
335 public Bounds3d transform(final Bounds3d boundingBox)
336 {
337 return new Bounds3d(transform(boundingBox.getPoints()));
338 }
339
340 /** {@inheritDoc} */
341 @Override
342 public int hashCode()
343 {
344 final int prime = 31;
345 int result = 1;
346 result = prime * result + Arrays.hashCode(this.mat);
347 return result;
348 }
349
350 /** {@inheritDoc} */
351 @Override
352 @SuppressWarnings("checkstyle:needbraces")
353 public boolean equals(final Object obj)
354 {
355 if (this == obj)
356 return true;
357 if (obj == null)
358 return false;
359 if (getClass() != obj.getClass())
360 return false;
361 Transform3d other = (Transform3d) obj;
362 if (!Arrays.equals(this.mat, other.mat))
363 return false;
364 return true;
365 }
366
367 /** {@inheritDoc} */
368 @Override
369 public String toString()
370 {
371 return "Transform3d [mat=" + Arrays.toString(this.mat) + "]";
372 }
373
374 }