Unofficial Dub Package Documentation 
1 /**
2  * Represents a 3D rotation of a given radians about an axis represented as an
3  * unit 3D vector.
4  * <p>
5  * This class uses double-precision components.
6  * 
7  * @author Kai Burjack
8  */
9 module doml.axis_angle_4d;
10 
11 import Math = doml.math;
12 
13 import doml.matrix_3d;
14 import doml.matrix_4d;
15 
16 
17 import doml.vector_3d;
18 import doml.vector_4d;
19 
20 import doml.quaternion_d;
21 
22 /*
23  * The MIT License
24  *
25  * Copyright (c) 2015-2021 Kai Burjack
26  %$%@^ Translated by jordan4ibanez
27  *
28  * Permission is hereby granted, free of charge, to any person obtaining a copy
29  * of this software and associated documentation files (the "Software"), to deal
30  * in the Software without restriction, including without limitation the rights
31  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
32  * copies of the Software, and to permit persons to whom the Software is
33  * furnished to do so, subject to the following conditions:
34  *
35  * The above copyright notice and this permission notice shall be included in
36  * all copies or substantial portions of the Software.
37  *
38  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
39  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
40  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
41  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
42  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
43  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
44  * THE SOFTWARE.
45  */
46 
47 /**
48  * Represents a 3D rotation of a given radians about an axis represented as an
49  * unit 3D vector.
50  * <p>
51  * This class uses double-precision components.
52  * 
53  * @author Kai Burjack
54  */
55 struct AxisAngle4d {
56     /**
57      * The angle in radians.
58      */
59     double angle = 0.0;
60     /**
61      * The x-component of the rotation axis.
62      */
63     double x = 0.0;
64     /**
65      * The y-component of the rotation axis.
66      */
67     double y = 0.0;
68     /**
69      * The z-component of the rotation axis.
70      */
71     double z = 1.0;
72     /**
73      * Create a new {@link AxisAngle4d} with the same values of <code>a</code>.
74      * 
75      * @param a
76      *            the AngleAxis4d to copy the values from
77      */
78     this(AxisAngle4d a) {
79         x = a.x;
80         y = a.y;
81         z = a.z;
82         angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
83     }
84 
85     /**
86      * Create a new {@link AxisAngle4d} from the given {@link Quaterniond}.
87      * <p>
88      * Reference: <a href=
89      * "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/"
90      * >http://www.euclideanspace.com</a>
91      * 
92      * @param q
93      *            the quaternion from which to create the new AngleAxis4d
94      */
95     this(Quaterniond q) {
96         double acos = Math.safeAcos(q.w);
97         double invSqrt = Math.invsqrt(1.0 - q.w * q.w);
98         if (Math.isInfinite(invSqrt)) {
99             this.x = 0.0;
100             this.y = 0.0;
101             this.z = 1.0;
102         } else {
103             this.x = q.x * invSqrt;
104             this.y = q.y * invSqrt;
105             this.z = q.z * invSqrt;
106         }
107         this.angle = acos + acos;
108     }
109 
110     /**
111      * Create a new {@link AxisAngle4d} with the given values.
112      *
113      * @param angle
114      *            the angle in radians
115      * @param x
116      *            the x-coordinate of the rotation axis
117      * @param y
118      *            the y-coordinate of the rotation axis
119      * @param z
120      *            the z-coordinate of the rotation axis
121      */
122     this(double angle, double x, double y, double z) {
123         this.x = x;
124         this.y = y;
125         this.z = z;
126         this.angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
127     }
128 
129     /**
130      * Create a new {@link AxisAngle4d} with the given values.
131      *
132      * @param angle the angle in radians
133      * @param v     the rotation axis as a {@link Vector3d}
134      */
135     this(double angle, Vector3d v) {
136         this(angle, v.x, v.y, v.z);
137     }
138 
139 
140     /**
141      * Set this {@link AxisAngle4d} to the given values.
142      * 
143      * @param angle
144      *            the angle in radians
145      * @param x
146      *            the x-coordinate of the rotation axis
147      * @param y
148      *            the y-coordinate of the rotation axis
149      * @param z
150      *            the z-coordinate of the rotation axis
151      * @return this
152      */
153     ref public AxisAngle4d set(double angle, double x, double y, double z) return {
154         this.x = x;
155         this.y = y;
156         this.z = z;
157         this.angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
158         return this;
159     }
160 
161     /**
162      * Set this {@link AxisAngle4d} to the given values.
163      *
164      * @param angle
165      *            the angle in radians
166      * @param v    
167      *            the rotation axis as a {@link Vector3d}
168      * @return this
169      */
170     ref public AxisAngle4d set(double angle, Vector3d v) return {
171         return set(angle, v.x, v.y, v.z);
172     }
173 
174     /**
175      * Set this {@link AxisAngle4d} to be equivalent to the given
176      * {@link Quaterniond}.
177      * 
178      * @param q
179      *            the quaternion to set this AngleAxis4d from
180      * @return this
181      */
182     ref public AxisAngle4d set(Quaterniond q) return {
183         double acos = Math.safeAcos(q.w);
184         double invSqrt = Math.invsqrt(1.0f - q.w * q.w);
185         if (Math.isInfinite(invSqrt)) {
186             this.x = 0.0;
187             this.y = 0.0;
188             this.z = 1.0;
189         } else {
190             this.x = q.x * invSqrt;
191             this.y = q.y * invSqrt;
192             this.z = q.z * invSqrt;
193         }
194         this.angle = acos + acos;
195         return this;
196     }
197 
198     /**
199      * Set this {@link AxisAngle4d} to be equivalent to the rotation 
200      * of the given {@link Matrix3d}.
201      * <p>
202      * Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
203      * 
204      * @param m
205      *            the Matrix3d to set this AngleAxis4d from
206      * @return this
207      */
208     ref public AxisAngle4d set(Matrix3d m) return {
209         double nm00 = m.m00, nm01 = m.m01, nm02 = m.m02;
210         double nm10 = m.m10, nm11 = m.m11, nm12 = m.m12;
211         double nm20 = m.m20, nm21 = m.m21, nm22 = m.m22;
212         double lenX = Math.invsqrt(m.m00 * m.m00 + m.m01 * m.m01 + m.m02 * m.m02);
213         double lenY = Math.invsqrt(m.m10 * m.m10 + m.m11 * m.m11 + m.m12 * m.m12);
214         double lenZ = Math.invsqrt(m.m20 * m.m20 + m.m21 * m.m21 + m.m22 * m.m22);
215         nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
216         nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
217         nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
218         double epsilon = 1E-4, epsilon2 = 1E-3;
219         if (Math.abs(nm10 - nm01) < epsilon && Math.abs(nm20 - nm02) < epsilon && Math.abs(nm21 - nm12) < epsilon) {
220             if (Math.abs(nm10 + nm01) < epsilon2 && Math.abs(nm20 + nm02) < epsilon2 && Math.abs(nm21 + nm12) < epsilon2
221                     && Math.abs(nm00 + nm11 + nm22 - 3) < epsilon2) {
222                 x = 0;
223                 y = 0;
224                 z = 1;
225                 angle = 0;
226                 return this;
227             }
228             angle = Math.PI;
229             double xx = (nm00 + 1) / 2;
230             double yy = (nm11 + 1) / 2;
231             double zz = (nm22 + 1) / 2;
232             double xy = (nm10 + nm01) / 4;
233             double xz = (nm20 + nm02) / 4;
234             double yz = (nm21 + nm12) / 4;
235             if ((xx > yy) && (xx > zz)) {
236                 x = Math.sqrt(xx);
237                 y = xy / x;
238                 z = xz / x;
239             } else if (yy > zz) {
240                 y = Math.sqrt(yy);
241                 x = xy / y;
242                 z = yz / y;
243             } else {
244                 z = Math.sqrt(zz);
245                 x = xz / z;
246                 y = yz / z;
247             }
248             return this;
249         }
250         double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
251         angle = Math.safeAcos((nm00 + nm11 + nm22 - 1) / 2);
252         x = (nm12 - nm21) / s;
253         y = (nm20 - nm02) / s;
254         z = (nm01 - nm10) / s;
255         return this;
256     }
257 
258     /**
259      * Set this {@link AxisAngle4d} to be equivalent to the rotational component 
260      * of the given {@link Matrix4d}.
261      * <p>
262      * Reference: <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/">http://www.euclideanspace.com</a>
263      * 
264      * @param m
265      *            the Matrix4d to set this AngleAxis4d from
266      * @return this
267      */
268     ref public AxisAngle4d set(Matrix4d m) return {
269         double nm00 = m.m00, nm01 = m.m01, nm02 = m.m02;
270         double nm10 = m.m10, nm11 = m.m11, nm12 = m.m12;
271         double nm20 = m.m20, nm21 = m.m21, nm22 = m.m22;
272         double lenX = Math.invsqrt(m.m00 * m.m00 + m.m01 * m.m01 + m.m02 * m.m02);
273         double lenY = Math.invsqrt(m.m10 * m.m10 + m.m11 * m.m11 + m.m12 * m.m12);
274         double lenZ = Math.invsqrt(m.m20 * m.m20 + m.m21 * m.m21 + m.m22 * m.m22);
275         nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
276         nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
277         nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
278         double epsilon = 1E-4, epsilon2 = 1E-3;
279         if (Math.abs(nm10 - nm01) < epsilon && Math.abs(nm20 - nm02) < epsilon && Math.abs(nm21 - nm12) < epsilon) {
280             if (Math.abs(nm10 + nm01) < epsilon2 && Math.abs(nm20 + nm02) < epsilon2 && Math.abs(nm21 + nm12) < epsilon2
281                     && Math.abs(nm00 + nm11 + nm22 - 3) < epsilon2) {
282                 x = 0;
283                 y = 0;
284                 z = 1;
285                 angle = 0;
286                 return this;
287             }
288             angle = Math.PI;
289             double xx = (nm00 + 1) / 2;
290             double yy = (nm11 + 1) / 2;
291             double zz = (nm22 + 1) / 2;
292             double xy = (nm10 + nm01) / 4;
293             double xz = (nm20 + nm02) / 4;
294             double yz = (nm21 + nm12) / 4;
295             if ((xx > yy) && (xx > zz)) {
296                 x = Math.sqrt(xx);
297                 y = xy / x;
298                 z = xz / x;
299             } else if (yy > zz) {
300                 y = Math.sqrt(yy);
301                 x = xy / y;
302                 z = yz / y;
303             } else {
304                 z = Math.sqrt(zz);
305                 x = xz / z;
306                 y = yz / z;
307             }
308             return this;
309         }
310         double s = Math.sqrt((nm12 - nm21) * (nm12 - nm21) + (nm20 - nm02) * (nm20 - nm02) + (nm01 - nm10) * (nm01 - nm10));
311         angle = Math.safeAcos((nm00 + nm11 + nm22 - 1) / 2);
312         x = (nm12 - nm21) / s;
313         y = (nm20 - nm02) / s;
314         z = (nm01 - nm10) / s;
315         return this;
316     }
317 
318     /**
319      * Set this {@link AxisAngle4d} to the values of <code>a</code>.
320      * 
321      * @param a
322      *            the AngleAxis4f to copy the values from
323      * @return this
324      */
325     ref public AxisAngle4d set(AxisAngle4d a) return {
326         x = a.x;
327         y = a.y;
328         z = a.z;
329         angle = (a.angle < 0.0 ? Math.PI + Math.PI + a.angle % (Math.PI + Math.PI) : a.angle) % (Math.PI + Math.PI);
330         return this;
331     }
332 
333     /**
334      * Set the given {@link Quaterniond} to be equivalent to this {@link AxisAngle4d} rotation.
335      * 
336      * @see Quaterniond#set(AxisAngle4d)
337      * 
338      * @param q
339      *          the quaternion to set
340      * @return q
341      */
342     public Quaterniond get(Quaterniond q) {
343         return q.set(this);
344     }
345 
346     /**
347      * Set the given {@link Matrix4d} to a rotation transformation equivalent to this {@link AxisAngle4d}.
348      * 
349      * @see Matrix4f#set(AxisAngle4d)
350      * 
351      * @param m
352      *          the matrix to set
353      * @return m
354      */
355     public Matrix4d get(Matrix4d m) {
356         return m.set(this);
357     }
358 
359     /**
360      * Set the given {@link Matrix3d} to a rotation transformation equivalent to this {@link AxisAngle4d}.
361      * 
362      * @see Matrix3f#set(AxisAngle4d)
363      * 
364      * @param m
365      *          the matrix to set
366      * @return m
367      */
368     public Matrix3d get(Matrix3d m) {
369         return m.set(this);
370     }
371 
372     /**
373      * Set the given {@link AxisAngle4d} to this {@link AxisAngle4d}.
374      * 
375      * @param dest
376      *          will hold the result
377      * @return dest
378      */
379     public AxisAngle4d get(AxisAngle4d dest) {
380         return dest.set(this);
381     }
382 
383     /**
384      * Normalize the axis vector.
385      * 
386      * @return this
387      */
388     ref public AxisAngle4d normalize() return {
389         double invLength = Math.invsqrt(x * x + y * y + z * z);
390         x *= invLength;
391         y *= invLength;
392         z *= invLength;
393         return this;
394     }
395 
396     /**
397      * Increase the rotation angle by the given amount.
398      * <p>
399      * This method also takes care of wrapping around.
400      * 
401      * @param ang
402      *          the angle increase
403      * @return this
404      */
405     ref public AxisAngle4d rotate(double ang) return {
406         angle += ang;
407         angle = (angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI);
408         return this;
409     }
410 
411     /**
412      * Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}.
413      * 
414      * @param v
415      *          the vector to transform
416      * @return v
417      */
418     public Vector3d transform(Vector3d v) {
419         return transform(v, v);
420     }
421 
422     /**
423      * Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}
424      * and store the result in <code>dest</code>.
425      * 
426      * @param v
427      *          the vector to transform
428      * @param dest
429      *          will hold the result
430      * @return dest
431      */
432     public Vector3d transform(Vector3d v, Vector3d dest) {
433         double sin = Math.sin(angle);
434         double cos = Math.cosFromSin(sin, angle);
435         double dot = x * v.x + y * v.y + z * v.z;
436         dest.set(v.x * cos + sin * (y * v.z - z * v.y) + (1.0 - cos) * dot * x,
437                  v.y * cos + sin * (z * v.x - x * v.z) + (1.0 - cos) * dot * y,
438                  v.z * cos + sin * (x * v.y - y * v.x) + (1.0 - cos) * dot * z);
439         return dest;
440     }
441 
442     /**
443      * Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}.
444      * 
445      * @param v
446      *          the vector to transform
447      * @return v
448      */
449     public Vector4d transform(Vector4d v) {
450         return transform(v, v);
451     }
452 
453     /**
454      * Transform the given vector by the rotation transformation described by this {@link AxisAngle4d}
455      * and store the result in <code>dest</code>.
456      * 
457      * @param v
458      *          the vector to transform
459      * @param dest
460      *          will hold the result
461      * @return dest
462      */
463     public Vector4d transform(Vector4d v, Vector4d dest) {
464         double sin = Math.sin(angle);
465         double cos = Math.cosFromSin(sin, angle);
466         double dot = x * v.x + y * v.y + z * v.z;
467         dest.set(v.x * cos + sin * (y * v.z - z * v.y) + (1.0 - cos) * dot * x,
468                  v.y * cos + sin * (z * v.x - x * v.z) + (1.0 - cos) * dot * y,
469                  v.z * cos + sin * (x * v.y - y * v.x) + (1.0 - cos) * dot * z,
470                  dest.w);
471         return dest;
472     }
473 
474     public int hashCode() {
475         immutable int prime = 31;
476         int result = 1;
477         long temp;
478         temp = Math.doubleToLongBits((angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI));
479         result = prime * result + cast(int) (temp ^ (temp >>> 32));
480         temp = Math.doubleToLongBits(x);
481         result = prime * result + cast(int) (temp ^ (temp >>> 32));
482         temp = Math.doubleToLongBits(y);
483         result = prime * result + cast(int) (temp ^ (temp >>> 32));
484         temp = Math.doubleToLongBits(z);
485         result = prime * result + cast(int) (temp ^ (temp >>> 32));
486         return result;
487     }
488 
489     public bool equals(AxisAngle4d other) {
490         if (this == other)
491             return true;
492         if (Math.doubleToLongBits((angle < 0.0 ? Math.PI + Math.PI + angle % (Math.PI + Math.PI) : angle) % (Math.PI + Math.PI)) != 
493                 Math.doubleToLongBits((other.angle < 0.0 ? Math.PI + Math.PI + other.angle % (Math.PI + Math.PI) : other.angle) % (Math.PI + Math.PI)))
494             return false;
495         if (Math.doubleToLongBits(x) != Math.doubleToLongBits(other.x))
496             return false;
497         if (Math.doubleToLongBits(y) != Math.doubleToLongBits(other.y))
498             return false;
499         if (Math.doubleToLongBits(z) != Math.doubleToLongBits(other.z))
500             return false;
501         return true;
502     }
503 }

Page generated by adrdox