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

Page generated by adrdox