unit_quaternion 0.0.2

Sign up to get free protection for your applications and to get access to all the features.
data/.gitignore ADDED
@@ -0,0 +1,6 @@
1
+ *~
2
+ #*
3
+ other/*
4
+ Gemfile.lock
5
+ pkg/*
6
+ coverage
data/Gemfile ADDED
@@ -0,0 +1,4 @@
1
+ source 'https://rubygems.org'
2
+
3
+ # Specify your gem's dependencies in unit_quaternion.gemspec
4
+ gemspec
data/LICENSE.txt ADDED
@@ -0,0 +1,25 @@
1
+ Copyright (c) 2016, Cory Crean
2
+ All rights reserved.
3
+
4
+ Redistribution and use in source and binary forms, with or without
5
+ modification, are permitted provided that the following conditions are met:
6
+ * Redistributions of source code must retain the above copyright
7
+ notice, this list of conditions and the following disclaimer.
8
+ * Redistributions in binary form must reproduce the above copyright
9
+ notice, this list of conditions and the following disclaimer in the
10
+ documentation and/or other materials provided with the distribution.
11
+ * The name(s) of the author(s) may not be used to endorse or promote
12
+ products derived from this software without specific prior written
13
+ permission.
14
+
15
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18
+ A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CORY CREAN BE
19
+ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20
+ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21
+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
22
+ BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
23
+ WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
24
+ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
25
+ IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
data/README.md ADDED
@@ -0,0 +1,103 @@
1
+ # Quaternion
2
+
3
+ This package provides a Quaternion class, along with a UnitQuaternion
4
+ class that is designed to be used to represent rotations.
5
+
6
+ ## Installation
7
+
8
+ This package is not available through any gem repository (yet).
9
+ Install it using:
10
+
11
+ $ sudo rake install
12
+
13
+ ## Usage
14
+
15
+ To use the Quaternion and UnitQuaternion classes in your program,
16
+ include the following line:
17
+
18
+ require 'unit_quaternion'
19
+
20
+ You can perform basic quaternion operations, such as addition and multiplication:
21
+
22
+ q1 = Quaternion.new(1,2,3,4)
23
+ => (1, Vector[2, 3, 4])
24
+
25
+ q2 = Quaternion.new(4,3,2,1)
26
+ => (4, Vector[3, 2, 1])
27
+
28
+ q1 + q2
29
+ => (5, Vector[5, 5, 5])
30
+
31
+ q1 * q2
32
+ => (-12, Vector[6, 24, 12])
33
+
34
+ You can use the UnitQuaternion class to represent spatial rotations.
35
+ The following represents a rotation of PI/2 radians about the x-axis:
36
+
37
+ qx = UnitQuaternion.fromAngleAxis(Math::PI/2, Vector[1, 0, 0])
38
+ => (0.7071067811865476, Vector[0.7071067811865475, 0.0, 0.0])
39
+
40
+ The following represents a rotation of PI/2 radians about the y-axis:
41
+
42
+ qy = UnitQuaternion.fromAngleAxis(Math::PI/2, Vector[0, 1, 0])
43
+ => (0.7071067811865476, Vector[0.0, 0.7071067811865475, 0.0])
44
+
45
+ You can use quaternion multiplication to compose rotations. If we
46
+ want to find the quaternion describing a rotation about the body-fixed
47
+ x-axis, followed by a rotation about the body-fixed y-axis, we would
48
+ do the following:
49
+
50
+ q = qx * qy
51
+ => (0.5000000000000001, Vector[0.5, 0.5, 0.4999999999999999])
52
+
53
+ Notice that this is the same as:
54
+
55
+ q = UnitQuaternion.fromEuler(Math::PI/2, Math::PI/2, 0, 'xyz')
56
+ => (0.5000000000000001, Vector[0.5, 0.5, 0.4999999999999999])
57
+
58
+ If we want to find the quaternion describing a rotation the inertial
59
+ x-axis, followed by a rotation about the inertial y-axis, we would do
60
+ the following:
61
+
62
+ q = qy * qx
63
+ => (0.5000000000000001, Vector[0.5, 0.5, -0.4999999999999999])
64
+
65
+ Notice that this is the same as:
66
+
67
+ q = UnitQuaternion.fromEuler(Math::PI/2, Math::PI/2, 0, 'XYZ')
68
+ => (0.5000000000000001, Vector[0.5, 0.5, -0.4999999999999999])
69
+
70
+ Additionally, you can use the method fromRotationMatrix to set the
71
+ values of the quaternion from an orthonormal 3x3 matrix. Finally, you
72
+ can use the methods getAngleAxis, getEuler, and getRotationMatrix to
73
+ find any of the corresponding representations of a spatial rotation
74
+ from a given quaternion.
75
+
76
+ The transform method takes a vector as an argument and returns the
77
+ result of rotating that vector through the rotation described by the
78
+ quaternion. For example:
79
+
80
+ q = UnitQuaternion.fromAngleAxis(Math::PI/2, Vector[0, 0, 1])
81
+ => (0.7071067811865476, Vector[0.0, 0.0, 0.7071067811865475])
82
+
83
+ v = Vector[1, 0, 0]
84
+ => Vector[1, 0, 0]
85
+
86
+ q.transform(v)
87
+ => Vector[2.220446049250313e-16, 1.0, 0.0]
88
+
89
+ gives the result of rotating the vector (1, 0, 0) through PI/2 radians
90
+ about the z-axis.
91
+
92
+ The inverse method returns the inverse of a given Quaternion. If we
93
+ have a UnitQuaternion q representing a rotation of theta radians about
94
+ the axis n, then q.inverse() represents a rotation of -theta about n
95
+ or, equivalently, a rotation of theta about -n.
96
+
97
+ ## Contributing
98
+
99
+ 1. Fork it
100
+ 2. Create your feature branch (`git checkout -b my-new-feature`)
101
+ 3. Commit your changes (`git commit -am 'Add some feature'`)
102
+ 4. Push to the branch (`git push origin my-new-feature`)
103
+ 5. Create new Pull Request
data/Rakefile ADDED
@@ -0,0 +1,5 @@
1
+ require "bundler/gem_tasks"
2
+
3
+ task :test do
4
+ ruby "tests/tests.rb"
5
+ end
data/lib/quaternion.rb ADDED
@@ -0,0 +1,127 @@
1
+ # Author:: Cory Crean (mailto:cory.crean@gmail.com)
2
+ # Copyright:: Copyright (c) 2016 Cory Crean
3
+ # License:: BSD
4
+ #
5
+ # A basic quaternion class, which implements standard operations such
6
+ # as addition, subtraction, multiplication, scalar division,
7
+ # inversion, conjugation, etc.
8
+
9
+ require 'matrix'
10
+
11
+ class Quaternion
12
+
13
+ # Create new quaternion from 4 values. If no arguments are
14
+ # provided, creates the zero quaternion.
15
+ def initialize(*args)
16
+ if args.length() == 4
17
+ set(*args)
18
+ elsif args.length() == 0
19
+ set(0, 0, 0, 0)
20
+ else
21
+ raise(ArgumentError, "wrong number of arguments (must be 0 or 4)")
22
+ end
23
+ end
24
+
25
+ # Set the quaternion's values.
26
+ #
27
+ # Params:
28
+ # +w+:: the real part of the quaterion
29
+ # +x+:: the i-component
30
+ # +y+:: the j-component
31
+ # +z+:: the k-component
32
+ def set(w, x, y, z)
33
+ @beta0 = w
34
+ @beta_s = Vector[x,y,z]
35
+ end
36
+
37
+ # Returns the quaternion's values as a scalar and a vector.
38
+ def get
39
+ return @beta0, @beta_s
40
+ end
41
+
42
+ # Returns the magnitude of the quaternion
43
+ def norm
44
+ return Math.sqrt(@beta0**2 + @beta_s.norm()**2)
45
+ end
46
+
47
+ # Returns the conjugate of the quaternion
48
+ def conjugate
49
+ return Quaternion.new(@beta0, *(-1*@beta_s))
50
+ end
51
+
52
+ # Returns the multiplicative inverse of the quaterion
53
+ def inverse
54
+ return self.conjugate() / self.norm() ** 2
55
+ end
56
+
57
+ # Returns a normalized quaternion. q.normalized() is equivalent to
58
+ # q/q.norm()
59
+ def normalized
60
+ return self / norm()
61
+ end
62
+
63
+ # Returns the sum of two quaternions
64
+ def +(q)
65
+ beta0, beta_s = q.get()
66
+ return Quaternion.new(@beta0 + beta0, *(@beta_s + beta_s))
67
+ end
68
+
69
+ # Returns the difference of two quaternions
70
+ def -(q)
71
+ beta0, beta_s = q.get()
72
+ return Quaternion.new(@beta0 - beta0, *(@beta_s - beta_s))
73
+ end
74
+
75
+ # Returns the additive inverse of the quaternion
76
+ def -@
77
+ Quaternion.new(-@beta0, -@beta_s[0], -@beta_s[1], -@beta_s[2])
78
+ end
79
+
80
+ # Returns the result of dividing the quaternion by a scalar
81
+ def /(s)
82
+ return Quaternion.new(@beta0 / s, *(@beta_s / s))
83
+ end
84
+
85
+ # Returns the result of multiplying the quaternion by a scalar or
86
+ # another quaternion
87
+ def *(q)
88
+ if q.is_a?(Numeric)
89
+ return Quaternion.new(@beta0 * q, *(@beta_s * q))
90
+ elsif q.is_a?(Quaternion)
91
+ q_beta0, q_beta_s = q.get()
92
+ beta0 = @beta0 * q_beta0 - @beta_s.inner_product(q_beta_s)
93
+ beta_s = @beta0 * q_beta_s + q_beta0 * @beta_s +
94
+ cross_product(@beta_s, q_beta_s)
95
+ result = self.class.new(beta0, *beta_s)
96
+ return result
97
+ end
98
+ end
99
+
100
+ # Returns true if two quaternions are equal (meaning that their
101
+ # corresponding entries are equal to each other) and false otherwise
102
+ def ==(q)
103
+ if get() == q.get()
104
+ return true
105
+ else
106
+ return false
107
+ end
108
+ end
109
+
110
+ # Returns the string representation of the quaternion
111
+ def to_s
112
+ return "(" + @beta0.to_s + ", " + @beta_s.to_s + ")"
113
+ end
114
+
115
+ def coerce(other)
116
+ return self, other
117
+ end
118
+
119
+ private
120
+ def cross_product(v1, v2)
121
+ # returns the cross product of vectors v1 and v2
122
+ return Vector[ v1[1]*v2[2] - v1[2]*v2[1],
123
+ v1[2]*v2[0] - v1[0]*v2[2],
124
+ v1[0]*v2[1] - v1[1]*v2[0] ]
125
+ end
126
+
127
+ end
@@ -0,0 +1,3 @@
1
+ module Quaternion
2
+ VERSION = "0.0.2"
3
+ end
@@ -0,0 +1,413 @@
1
+ # Author:: Cory Crean (mailto:cory.crean@gmail.com)
2
+ # Copyright:: Copyright (c) 2016 Cory Crean
3
+ # License:: BSD
4
+ #
5
+ # A unit quaternion class, designed to represent spatial rotations.
6
+ # Can convert between common representations of spatial rotations,
7
+ # such as angle-axis, Euler angles, and rotation matrices.
8
+
9
+ require 'matrix'
10
+ require_relative 'quaternion'
11
+
12
+ class UnitQuaternion < Quaternion
13
+
14
+ # Creates a new UnitQuaternion from 4 values. If the resulting
15
+ # quaternion does not have magnitude 1, it will be normalized. If
16
+ # no arguments are provided, creates the quaternion (1, 0, 0, 0).
17
+ def initialize(*args)
18
+ if args.length() == 4
19
+ set(*args)
20
+ elsif args.length() == 0
21
+ super(1, 0, 0, 0)
22
+ else
23
+ raise(ArgumentError, "wrong number of arguments (must be 0 or 4)")
24
+ end
25
+ end
26
+
27
+ # Initializes a quaternion from the angle-axis representation of a
28
+ # rotation. The sense of the rotation is determined according to
29
+ # the right-hand rule.
30
+ #
31
+ # Params:
32
+ # +angle+:: A scalar representing the angle of the rotation (in radians)
33
+ # +axis+:: A vector representing the axis of rotation (need not be a unit vector)
34
+ def self.fromAngleAxis(angle, axis)
35
+ # intializes a quaternion from the angle-axis representation of a
36
+ # rotation
37
+ q = UnitQuaternion.new()
38
+ q.setAngleAxis(angle, axis)
39
+ return q
40
+ end
41
+
42
+ # Initializes a quaternion from a set of 3 Euler angles.
43
+ #
44
+ # Params:
45
+ # +theta1+:: The angle of rotation about the first axis
46
+ # +theta2+:: The angle of rotation about the second axis
47
+ # +theta3+:: The angle of rotation about the third axis
48
+ # +axes+:: A string of 3 letters ('X/x', 'Y/y', or 'Z/z') representing the three axes of rotation. Must be all uppercase or all lowercase. If the string is uppercase, the rotations are performed about the inertial axes. If the string is lowercase, the rotations are performed about the body-fixed axes. Repeated axes are allowed, but not in succession (for example, 'xyx' is fine, but 'xxy' is not allowed).
49
+ def self.fromEuler(theta1, theta2, theta3, axes)
50
+ q = UnitQuaternion.new()
51
+ q.setEuler(theta1, theta2, theta3, axes)
52
+ return q
53
+ end
54
+
55
+ # Initializes a quaternion from a rotation matrix
56
+ #
57
+ # Params:
58
+ # +mat+:: A 3x3 orthonormal matrix.
59
+ def self.fromRotationMatrix(mat)
60
+ q = UnitQuaternion.new()
61
+ q.setRotationMatrix(mat)
62
+ return q
63
+ end
64
+
65
+ # Set the quaternion's values. If the 4 arguments do not form an
66
+ # unit quaternion, the resulting quaternion is normalized.
67
+ #
68
+ # Params:
69
+ # +w+:: the real part of the quaterion
70
+ # +x+:: the i-component
71
+ # +y+:: the j-component
72
+ # +z+:: the k-component
73
+ def set(w, x, y, z)
74
+ # sets the values of the quaternion
75
+ super(w, x, y, z)
76
+ @beta0, @beta_s = normalized().get()
77
+ end
78
+
79
+ # Sets the values of the quaternion from the angle-axis
80
+ # representation of a rotation. The sense of the rotation is
81
+ # determined according to the right-hand rule.
82
+ #
83
+ # Params:
84
+ # +angle+:: A scalar representing the angle of the rotation (in radians)
85
+ # +axis+:: A vector representing the axis of rotation (need not be a unit vector)
86
+ def setAngleAxis(angle, axis)
87
+ # sets the quaternion based on the angle-axis representation of a
88
+ # rotation
89
+ if axis == Vector[0,0,0]
90
+ raise(ArgumentError, "Axis must not be the zero vector")
91
+ end
92
+ if axis.size() != 3
93
+ raise(ArgumentError, "Axis must be a 3-dimensional vector")
94
+ end
95
+ axis = axis.normalize()
96
+ @beta0 = Math.cos(angle / 2.0)
97
+ beta1 = axis[0] * Math.sin(angle / 2.0)
98
+ beta2 = axis[1] * Math.sin(angle / 2.0)
99
+ beta3 = axis[2] * Math.sin(angle / 2.0)
100
+ @beta_s = Vector[beta1, beta2, beta3]
101
+ end
102
+
103
+ # Returns the angle-axis representation of the rotation represented
104
+ # by the quaternion.
105
+ #
106
+ # Returns:
107
+ # +angle+:: A scalar representing the angle of rotation (in radians)
108
+ # +axis+:: A unit vector representing the axis of rotation
109
+ def getAngleAxis
110
+ # return the angle-axis representation of the rotation contained in
111
+ # this quaternion
112
+ angle = 2*Math.acos(@beta0)
113
+
114
+ # if sin(theta/2) = 0, then theta = 2*n*PI, where n is any integer,
115
+ # which means that the object has performed a complete rotation, and
116
+ # any axis will do
117
+ if Math.sin(angle/2).abs() < 1e-15
118
+ axis = Vector[1, 0, 0]
119
+ else
120
+ axis = @beta_s / Math.sin(angle/2)
121
+ end
122
+ return angle, axis
123
+ end
124
+
125
+ # Sets the values of the quaternion from a set of 3 Euler angles.
126
+ #
127
+ # Params:
128
+ # +theta1+:: The angle of rotation about the first axis
129
+ # +theta2+:: The angle of rotation about the second axis
130
+ # +theta3+:: The angle of rotation about the third axis
131
+ # +axes+:: A string of 3 letters ('X/x', 'Y/y', or 'Z/z') representing the three axes of rotation. Must be all uppercase or all lowercase. If the string is uppercase, the rotations are performed about the inertial axes. If the string is lowercase, the rotations are performed about the body-fixed axes. Repeated axes are allowed, but not in succession (for example, 'xyx' is fine, but 'xxy' is not allowed).
132
+ def setEuler(theta1, theta2, theta3, axes)
133
+ if axes.length() != 3
134
+ raise(ArgumentError, "Must specify exactly 3 axes")
135
+ end
136
+ quats = Array.new(3)
137
+ theta = [theta1, theta2, theta3]
138
+ for i in 0..2
139
+ if axes.upcase()[i] == 'X'
140
+ quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[1, 0, 0])
141
+ elsif axes.upcase()[i] == 'Y'
142
+ quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[0, 1, 0])
143
+ elsif axes.upcase()[i] == 'Z'
144
+ quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[0, 0, 1])
145
+ else
146
+ raise(ArgumentError, "Axes can only be X/x, Y/y, or Z/z")
147
+ end
148
+ end
149
+ if axes == axes.upcase()
150
+ @beta0, @beta_s = (quats[2] * quats[1] * quats[0]).get()
151
+ elsif axes == axes.downcase()
152
+ @beta0, @beta_s = (quats[0] * quats[1] * quats[2]).get()
153
+ else
154
+ raise(ArgumentError, "Axes must be either all uppercase or all " +
155
+ "lowercase")
156
+ end
157
+ end
158
+
159
+ # Returns the Euler angles corresponding to this quaternion.
160
+ #
161
+ # Params:
162
+ # +axes+:: A string of 3 letters ('X/x', 'Y/y', or 'Z/z') representing the three axes of rotation. Must be all uppercase or all lowercase. If the string is uppercase, the rotations are performed about the inertial axes. If the string is lowercase, the rotations are performed about the body-fixed axes. Repeated axes are allowed, but not in succession (for example, 'xyx' is fine, but 'xxy' is not allowed).
163
+ #
164
+ # Returns:
165
+ # +theta1+:: The angle of rotation about the first axis
166
+ # +theta2+:: The angle of rotation about the second axis
167
+ # +theta3+:: The angle of rotation about the third axis
168
+ def getEuler(axes)
169
+ # Returns the Euler angles about the specified axes. The axes should
170
+ # be specified as a string, and can be any permutation (with
171
+ # replacement) of 'X', 'Y', and 'Z', as long as no letter is adjacent
172
+ # to itself (for example, 'XYX' is valid, but 'XXY' is not).
173
+ # If the axes are uppercase, this function returns the angles about
174
+ # the global axes. If they are lowercase, this function returns the
175
+ # angles about the body-fixed axes.
176
+ #
177
+ # This method implements Shoemake's algorithm for finding the Euler
178
+ # angles from a rotation matrix, found in Graphics Gems IV (pg. 222).
179
+
180
+ if axes.length() != 3
181
+ raise(ArgumentError, "Exactly 3 axes must be specified in order to " +
182
+ "calculate the Euler angles")
183
+ end
184
+
185
+ if axes == axes.upcase()
186
+ # get angles about global axes
187
+ static = true
188
+ elsif axes == axes.downcase()
189
+ # get angles about body-fixes axes
190
+ static = false
191
+ axes = axes.reverse()
192
+ else
193
+ raise(ArgumentError, "Axes must either be all uppercase or all " +
194
+ "lowercase")
195
+ end
196
+
197
+ axes = axes.upcase()
198
+ if axes[0] == axes[2]
199
+ same = true
200
+ end
201
+
202
+ if not ('XYZ'.include?(axes[0]) and 'XYZ'.include?(axes[1]) and
203
+ 'XYZ'.include?(axes[2]) )
204
+ raise(ArgumentError, 'Axes can only be X/x, Y/y, or Z/z')
205
+ end
206
+
207
+ if axes.include?('XX') or axes.include?('YY') or
208
+ axes.include?('ZZ')
209
+ raise(ArgumentError, "Cannot rotate about the same axis twice in " +
210
+ "succession")
211
+ end
212
+
213
+ # true if the axes specify a right-handed coordinate system, false
214
+ # otherwise
215
+ rh = isRightHanded(axes.upcase())
216
+
217
+ p_mat_rows = Array.new()
218
+ unused = [ 'X', 'Y', 'Z' ]
219
+ axes[0..1].each_char() do |a|
220
+ if a == 'X'
221
+ p_mat_rows << getUnitVector(a)
222
+ elsif a == 'Y'
223
+ p_mat_rows << getUnitVector(a)
224
+ elsif a == 'Z'
225
+ p_mat_rows << getUnitVector(a)
226
+ end
227
+ unused.delete(a)
228
+ end
229
+ p_mat_rows << getUnitVector(unused[0])
230
+
231
+ p_mat = Matrix.rows(p_mat_rows)
232
+ rot_mat = p_mat * getRotationMatrix() * p_mat.transpose()
233
+
234
+ theta1, theta2, theta3 = parseMatrix(rot_mat, same)
235
+
236
+ if not static
237
+ theta1, theta3 = theta3, theta1
238
+ end
239
+
240
+ if not rh
241
+ theta1, theta2, theta3 = -theta1, -theta2, -theta3
242
+ end
243
+
244
+ return theta1, theta2, theta3
245
+ end
246
+
247
+ # Sets the values of the quaternion from a rotation matrix
248
+ #
249
+ # Params:
250
+ # +mat+:: A 3x3 orthonormal matrix.
251
+ def setRotationMatrix(mat)
252
+ if mat.row_size() != 3 or mat.column_size() != 3
253
+ raise(ArgumentError, "Rotation matrix must be 3x3")
254
+ end
255
+ tol = 1e-15
256
+ if not isOrthonormalMatrix(mat, tol)
257
+ raise(ArgumentError, "Matrix is not orthonormal (to within " +
258
+ tol.to_s(), ")")
259
+ end
260
+ theta1, theta2, theta3 = parseMatrix(mat, false)
261
+ setEuler(theta1, theta2, theta3, 'XYZ')
262
+ end
263
+
264
+ # Returns the rotation matrix corresponding to this quaternion.
265
+ def getRotationMatrix
266
+ # returns the rotation matrix corresponding to this quaternion
267
+ return Matrix[ [ @beta0**2 + @beta_s[0]**2 - @beta_s[1]**2 - @beta_s[2]**2,
268
+ 2*(@beta_s[0]*@beta_s[1] - @beta0*@beta_s[2]),
269
+ 2*(@beta_s[0]*@beta_s[2] + @beta0*@beta_s[1]) ],
270
+ [ 2*(@beta_s[0]*@beta_s[1] + @beta0*@beta_s[2]),
271
+ @beta0**2 - @beta_s[0]**2 + @beta_s[1]**2 - @beta_s[2]**2,
272
+ 2*(@beta_s[1]*@beta_s[2] - @beta0*@beta_s[0]) ],
273
+ [ 2*(@beta_s[0]*@beta_s[2] - @beta0*@beta_s[1]),
274
+ 2*(@beta0*@beta_s[0] + @beta_s[1]*@beta_s[2]),
275
+ @beta0**2 - @beta_s[0]**2 - @beta_s[1]**2 + @beta_s[2]**2 ] ]
276
+ end
277
+
278
+ # Transforms a vector by applying to it the rotation represented by
279
+ # this quaternion, and returns the result.
280
+ #
281
+ # Params:
282
+ # +vec+:: A 3-D vector in the unrotated frame.
283
+ def transform(vec)
284
+ return getRotationMatrix() * vec
285
+ end
286
+
287
+ # Returns the inverse of the quaternion
288
+ def inverse
289
+ result = UnitQuaternion.new
290
+ result.set(@beta0, *(-1*@beta_s))
291
+ return result
292
+ end
293
+
294
+ private
295
+ def isRightHanded(axes)
296
+ if axes.length() != 3
297
+ raise(ArgumentError, "Only 3 axes permitted")
298
+ end
299
+ axes == axes.upcase()
300
+ if axes[0..1] == "XY" or axes[0..1] == "YZ" or axes[0..1] == "ZX"
301
+ return true
302
+ else
303
+ return false
304
+ end
305
+ end
306
+
307
+ def getUnitVector(axis)
308
+ axis = axis.upcase()
309
+ if axis == 'X'
310
+ return Vector[1, 0, 0]
311
+ elsif axis == 'Y'
312
+ return Vector[0, 1, 0]
313
+ elsif axis == 'Z'
314
+ return Vector[0, 0, 1]
315
+ else
316
+ raise(ArgumentError, "Axis can only be X/x, Y/y, or Z/z")
317
+ end
318
+ end
319
+
320
+ def parseMatrix(rot_mat, same)
321
+ # Extracts the Euler angles corresponding to the given matrix. If
322
+ # same = false, this method returns the angles about the global X,
323
+ # Y, and Z axes (in that order). If same = true, this method
324
+ # returns the Euler angles about the global X, Y, and X axes (in
325
+ # that order).
326
+ tol = 1e-15
327
+ if same
328
+ # print("rot_mat = ", rot_mat, "\n")
329
+ begin
330
+ theta2 = Math.acos(rot_mat[0,0])
331
+ rescue Math::DomainError
332
+ # the value of rot_mat[0,0] may be off slightly due to truncation
333
+ # error
334
+ if rot_mat[0,0] > 0
335
+ theta2 = 0
336
+ else
337
+ theta2 = Math::PI
338
+ end
339
+ end
340
+ if Math.sin(theta2).abs() < Math.sqrt(tol)
341
+ # if sin(theta2) is 0, then the first and third axes are
342
+ # either parallel or antiparallel, so we can only find the sum
343
+ # theta3 + theta1, not the individual angles. Here, we choose
344
+ # theta3 = 0 and solve for theta1.
345
+ theta3 = 0
346
+ y = rot_mat[2,1]
347
+ x = rot_mat[1,1]
348
+ sign = Math.cos(theta2) <=> 0
349
+ theta1 = Math.atan2(sign * y, x)
350
+ else
351
+ sign = Math.sin(theta2) <=> 0
352
+ theta1 = Math.atan2(sign * rot_mat[0,1], sign * rot_mat[0,2])
353
+ theta3 = Math.atan2(sign * rot_mat[1,0], -sign * rot_mat[2,0])
354
+ end
355
+ else
356
+ begin
357
+ theta2 = Math.asin(-rot_mat[2,0])
358
+ rescue Math::DomainError
359
+ # the value of rot_mat[2,0] may be off slightly due to truncation
360
+ # error
361
+ if -rot_mat[2,0] > 0
362
+ theta2 = Math::PI/2
363
+ else
364
+ theta2 = -Math::PI/2
365
+ end
366
+ end
367
+ if Math.cos(theta2).abs() < Math.sqrt(tol)
368
+ # if cos(theta2) is 0, then the first and third axes are
369
+ # either parallel or antiparallel, so we can only find the sum
370
+ # theta3 + theta1. Here, we choose theta3 = 0 and solve for
371
+ # theta1.
372
+ y = -rot_mat[1,2]
373
+ x = rot_mat[0,2]
374
+ sign = Math.sin(theta2) <=> 0
375
+ theta1 = Math.atan2(y, sign * x)
376
+ theta3 = 0
377
+ else
378
+ sign = Math.cos(theta2) <=> 0
379
+ theta1 = Math.atan2(sign * rot_mat[2,1], sign * rot_mat[2,2])
380
+ theta3 = Math.atan2(sign * rot_mat[1,0], sign * rot_mat[0,0])
381
+ end
382
+ end
383
+
384
+ return theta1, theta2, theta3
385
+ end
386
+
387
+ def isOrthonormalMatrix(mat, tol)
388
+ # Determines if mat is orthonormal. That is, determines whether
389
+ # mat.transpose() * mat is equal to the identity matrix (to within
390
+ # tol).
391
+
392
+ n_rows = mat.row_size()
393
+ n_cols = mat.column_size()
394
+ if (n_rows != n_cols)
395
+ return false
396
+ end
397
+ result = mat.transpose() * mat
398
+ for i in (0...n_rows)
399
+ for j in (0...n_cols)
400
+ if i == j
401
+ if result[i,j] - 1.abs() > tol
402
+ return false
403
+ end
404
+ else
405
+ if result[i,j] > tol
406
+ return false
407
+ end
408
+ end
409
+ end
410
+ end
411
+ return true
412
+ end
413
+ end
data/tests/tests.rb ADDED
@@ -0,0 +1,12 @@
1
+ # Name: tests.rb
2
+ # Description: Runs all test cases for both the Quaternion and
3
+ # UnitQuaternion class, and produces code coverage information.
4
+ # Author: Cory Crean
5
+ # E-mail: cory.crean@gmail.com
6
+ # Copyright (c) 2016, Cory Crean
7
+
8
+ require 'simplecov'
9
+ SimpleCov.start
10
+ require 'test/unit'
11
+ require_relative 'tests_Quaternion'
12
+ require_relative 'tests_UnitQuaternion'
@@ -0,0 +1,157 @@
1
+ # Name: tests_Quaternion.rb
2
+ # Description: Test cases for the Quaternion class.
3
+ # Author: Cory Crean
4
+ # E-mail: cory.crean@gmail.com
5
+ # Copyright (c) 2016, Cory Crean
6
+
7
+ require 'test/unit'
8
+ require 'matrix'
9
+ require_relative '../lib/quaternion'
10
+
11
+ class TestQuaternion < Test::Unit::TestCase
12
+
13
+ def setup
14
+ @quats = [ ::Quaternion.new(1,2,3,4),
15
+ ::Quaternion.new(0.1, 0.01, 2.3, 4),
16
+ ::Quaternion.new(1234.4134, 689.6124, 134.124, 0.5),
17
+ ::Quaternion.new(1,1,1,1),
18
+ ]
19
+ nums = (0..1).step(0.2).to_a + (2..10).step(2).to_a
20
+ nums.product(nums, nums, nums).each() do |w, x, y, z|
21
+ @quats << Quaternion.new(w, x, y, z)
22
+ end
23
+ end
24
+
25
+ def test_initialize
26
+ q = ::Quaternion.new(1,1,1,1)
27
+ beta0, beta_s = q.get()
28
+ assert_equal(1, beta0)
29
+ assert_equal(Vector[1,1,1], beta_s)
30
+
31
+ q = ::Quaternion.new
32
+ beta0, beta_s = q.get()
33
+ assert_equal(0, beta0)
34
+ assert_equal(Vector[0,0,0], beta_s)
35
+
36
+ vals = [ [ 1 ], [ 1, 2 ], [ 1, 2, 3, 4, 5 ], [ 1, 2, 3, 4, 5, 6 ] ]
37
+ for v in vals
38
+ assert_raise(ArgumentError) do
39
+ q = ::Quaternion.new(*v)
40
+ end
41
+ end
42
+ end
43
+
44
+ def test_set
45
+ q = ::Quaternion.new(0,0,0,0)
46
+ beta0, beta_s = q.get()
47
+ assert_equal(0, beta0)
48
+ assert_equal(Vector[0,0,0], beta_s)
49
+
50
+ q.set(1,2,3,4)
51
+ beta0, beta_s = q.get()
52
+ assert_equal(1, beta0)
53
+ assert_equal(Vector[2,3,4], beta_s)
54
+ end
55
+
56
+ def test_norm
57
+ q = ::Quaternion.new(1, 0, 0, 0)
58
+ assert_equal(1, q.norm())
59
+
60
+ q = ::Quaternion.new(1, 2, 3, 4)
61
+ assert_equal(Math.sqrt(1**2 + 2**2 + 3**2 + 4**2), q.norm())
62
+
63
+ assert_equal(0, ::Quaternion.new(0,0,0,0).norm())
64
+
65
+ for q in @quats
66
+ assert_in_delta(q.norm(), Math.sqrt((q*q.conjugate()).get[0]), 1e-14)
67
+ end
68
+ end
69
+
70
+ def test_conjugate_multiply
71
+ # tests the conjugate and * methods
72
+ q = ::Quaternion.new(1, 1, 3, 1)
73
+ q_c = q.conjugate()
74
+ beta0, beta_s = q_c.get()
75
+ assert_equal(1, beta0)
76
+ assert_equal(Vector[-1,-3,-1], beta_s)
77
+
78
+ for q in @quats
79
+ assert_in_delta((q*q.conjugate()).get[1].norm(), 0, 1e-15)
80
+ end
81
+ end
82
+
83
+ def test_inverse
84
+ @quats.delete(Quaternion.new(0,0,0,0))
85
+ for q in @quats
86
+ q_inv = q.inverse()
87
+ q_result = q * q_inv
88
+ beta0, beta_s = q_result.get()
89
+ assert_in_delta(1, beta0, 1e-15)
90
+ assert_in_delta(beta_s.norm(), 0, 1e-15)
91
+ end
92
+ end
93
+
94
+ def test_add
95
+ for q, q2 in @quats.zip(@quats)
96
+ sum = q + q2
97
+ assert_in_delta(sum.get()[0], q.get()[0] + q2.get()[0], 1e-15)
98
+ assert_in_delta((sum.get()[1] - (q.get()[1] + q2.get()[1])).norm(),
99
+ 1e-15)
100
+ end
101
+ end
102
+
103
+ def test_subtract
104
+ for q, q2 in @quats.zip(@quats)
105
+ sum = q - q2
106
+ assert_in_delta(sum.get()[0], q.get()[0] - q2.get()[0], 1e-15)
107
+ assert_in_delta((sum.get()[1] - (q.get()[1] - q2.get()[1])).norm(),
108
+ 1e-15)
109
+ end
110
+ end
111
+
112
+ def test_normalized
113
+ q = ::Quaternion.new(1,1,1,1)
114
+ beta0, beta_s = q.normalized().get()
115
+ assert_in_delta(0.5, beta0, 1e-15)
116
+ assert_in_delta((Vector[0.5,0.5,0.5] - beta_s).norm(), 0, 1e-15)
117
+
118
+ @quats.delete(Quaternion.new(0,0,0,0))
119
+ for q in @quats
120
+ assert_in_delta(q.normalized().norm(), 1, 1e-15)
121
+ end
122
+ end
123
+
124
+ def test_equality
125
+ for q in @quats
126
+ assert_equal(q, q)
127
+ end
128
+
129
+ assert(Quaternion.new(1,2,3,4) != Quaternion.new(4,3,2,1))
130
+ assert(Quaternion.new(0,0,0,0) != Quaternion.new(1,1,1,1))
131
+ end
132
+
133
+ def test_scalarMult
134
+ q = ::Quaternion.new(1,1,1,1)
135
+ assert_equal(3 * q, Quaternion.new(3,3,3,3))
136
+ assert_equal(1.111 * q, Quaternion.new(1.111, 1.111, 1.111, 1.111))
137
+ assert_equal(q * 3, Quaternion.new(3,3,3,3))
138
+ assert_equal(q * 1.111, Quaternion.new(1.111, 1.111, 1.111, 1.111))
139
+ end
140
+
141
+ def test_string
142
+ q = Quaternion.new(1,1,1,1)
143
+ assert_equal(q.to_s,
144
+ "(1, Vector[1, 1, 1])")
145
+
146
+ q = Quaternion.new(1,2,3,4)
147
+ assert_equal(q.to_s,
148
+ "(1, Vector[2, 3, 4])")
149
+ end
150
+
151
+ def test_unaryMinus
152
+ for q in @quats
153
+ beta0, beta_s = q.get()
154
+ assert_equal(-q, Quaternion.new(-beta0, -beta_s[0], -beta_s[1], -beta_s[2]))
155
+ end
156
+ end
157
+ end
@@ -0,0 +1,337 @@
1
+ # Name: tests_UnitQuaternion.rb
2
+ # Description: Test cases for the UnitQuaternion class.
3
+ # Author: Cory Crean
4
+ # E-mail: cory.crean@gmail.com
5
+ # Copyright (c) 2016, Cory Crean
6
+
7
+ require 'test/unit'
8
+ require 'matrix'
9
+ require_relative '../lib/unit_quaternion'
10
+
11
+ def areEqualMatrices(m1, m2, tol)
12
+ m1.zip(m2).each() do |v1, v2|
13
+ if (v1 - v2).abs() > tol
14
+ return false
15
+ end
16
+ end
17
+ return true
18
+ end
19
+
20
+ def isIdentityMatrix(m, tol)
21
+ for i, j in [0,1,2].product([0,1,2])
22
+ if i == j
23
+ if (m[i,j] - 1).abs > tol
24
+ return false
25
+ end
26
+ else
27
+ if m[i,j].abs > tol
28
+ return false
29
+ end
30
+ end
31
+ end
32
+ return true
33
+ end
34
+
35
+ class TestUnitQuaternion < Test::Unit::TestCase
36
+
37
+ def setup
38
+ @quats = [ ::UnitQuaternion.new(1,2,3,4),
39
+ ::UnitQuaternion.new(0.1, 0.01, 2.3, 4),
40
+ ::UnitQuaternion.new(1234.4134, 689.6124, 134.124, 0.5),
41
+ ::UnitQuaternion.new(1,1,1,1),
42
+ ]
43
+ @angles = [ 2*Math::PI, Math::PI, Math::PI/2, Math::PI/4,
44
+ 0.5, 0.25, 0.1234, 0, ]
45
+ # @angles = (0..2*Math::PI).step(0.2).to_a
46
+ # @angles << 2*Math::PI
47
+ # for i in 1..8
48
+ # @angles << Math::PI/i
49
+ # end
50
+ @axes = [ Vector[ 1, 1, 1 ], Vector[ 1, 0, 0 ], Vector[ 0, 1, 0 ],
51
+ Vector[ 0, 0, 1 ], Vector[ 1, 2, 3 ], ]
52
+ # @euler = []
53
+ # ['x', 'y', 'z'].permutation() do |s|
54
+ # @euler << s.join("")
55
+ # @euler << s.join("").upcase()
56
+ # end
57
+ @euler = ['XYZ', 'XZY', 'XYX', 'XZX', 'YXZ', 'YZX', 'YXY', 'YZY',
58
+ 'ZXY', 'ZYX', 'ZXZ', 'ZYZ', 'xyz', 'xzy', 'xyx', 'xzx',
59
+ 'yxz', 'yzx', 'yxy', 'yzy', 'zxy', 'zyx', 'zxz', 'zyz']
60
+ end
61
+
62
+ def test_initialize
63
+ q = ::UnitQuaternion.new(0.9848857801796105, 0.1, 0.1, 0.1)
64
+ beta0, beta_s = q.get()
65
+ assert_equal(0.9848857801796105, beta0)
66
+ assert_equal(Vector[0.1, 0.1, 0.1], beta_s)
67
+ assert_equal(1, beta0**2 + beta_s.norm()**2)
68
+
69
+ assert_raise(ArgumentError) do
70
+ q = ::UnitQuaternion.new(1,1,1)
71
+ end
72
+
73
+ q = ::UnitQuaternion.new(1, 0, 0, 0)
74
+ beta0, beta_s = q.get()
75
+ assert_equal(1, beta0)
76
+ assert_equal(Vector[0,0,0], beta_s)
77
+
78
+ q = ::UnitQuaternion.new(1, 1, 1, 1)
79
+ beta0, beta_s = q.get()
80
+ assert_in_delta(0.5, beta0, 1e-15)
81
+ assert_in_delta((Vector[0.5, 0.5, 0.5] - beta_s).norm(), 0, 1e-15)
82
+ end
83
+
84
+ def test_set
85
+ q = ::UnitQuaternion.new
86
+ q.set(0.9848857801796105, 0.1, 0.1, 0.1)
87
+ beta0, beta_s = q.get()
88
+ assert_equal(0.9848857801796105, beta0)
89
+ assert_equal(Vector[0.1, 0.1, 0.1], beta_s)
90
+ assert_equal(1, beta0**2 + beta_s.norm()**2)
91
+
92
+ assert_raise(ArgumentError) do
93
+ q.set(1,1,1)
94
+ end
95
+
96
+ q.set(1, 0, 0, 0)
97
+ beta0, beta_s = q.get()
98
+ assert_equal(1, beta0)
99
+ assert_equal(Vector[0,0,0], beta_s)
100
+
101
+ q.set(1, 1, 1, 1)
102
+ beta0, beta_s = q.get()
103
+ assert_in_delta(0.5, beta0, 1e-15)
104
+ assert_in_delta((Vector[0.5, 0.5, 0.5] - beta_s).norm(), 0, 1e-15)
105
+ end
106
+
107
+ def test_setAngleAxis
108
+ axis = Vector[1, 0, 0]
109
+ angle = Math::PI/2
110
+ q = ::UnitQuaternion.new
111
+ q.setAngleAxis(angle, axis)
112
+ beta0, beta_s = q.get()
113
+ assert_equal(Math.cos(angle/2.0), beta0)
114
+ assert_equal(axis[0]*Math.sin(angle/2.0), beta_s[0])
115
+ assert_equal(axis[1]*Math.sin(angle/2.0), beta_s[1])
116
+ assert_equal(axis[2]*Math.sin(angle/2.0), beta_s[2])
117
+
118
+ for angle, axis in @angles.product(@axes)
119
+ q = ::UnitQuaternion.new
120
+ q.setAngleAxis(angle, axis)
121
+ beta0, beta_s = q.get()
122
+ assert_in_delta(Math.cos(angle/2), beta0, 1e-15)
123
+ assert_in_delta((beta_s - axis.normalize()*Math.sin(angle/2)).norm(),
124
+ 0, 1e-15)
125
+ end
126
+
127
+ q2 = ::UnitQuaternion.new
128
+ assert_raise(ArgumentError) do
129
+ q2.setAngleAxis(0, Vector[0,0,0])
130
+ end
131
+ assert_raise(ArgumentError) do
132
+ q2.setAngleAxis(0, Vector[1,1,1,1])
133
+ end
134
+ assert_raise(ArgumentError) do
135
+ q2 = ::UnitQuaternion.fromAngleAxis(0, Vector[0,0,0])
136
+ end
137
+ assert_raise(ArgumentError) do
138
+ q2 = ::UnitQuaternion.fromAngleAxis(0, Vector[1,1,1,1])
139
+ end
140
+ end
141
+
142
+ def test_getAngleAxis
143
+ axis = Vector[1, 2, 3]
144
+ angle = 0.4321
145
+ q = ::UnitQuaternion.fromAngleAxis(angle, axis)
146
+ result_angle, result_axis = q.getAngleAxis()
147
+
148
+ assert_in_delta((axis.normalize() - result_axis).norm(), 0, 1e-15)
149
+ assert_in_delta(angle, result_angle, 1e-15)
150
+
151
+ # The angle-axis representation of a rotation is not unique. We could
152
+ # reverse the sign of both the angle and axis, or when the angle is a
153
+ # multiple of 2*PI, any axis will do. Therefore, we compare rotation
154
+ # matrices instead of angles and axes.
155
+ for angle, axis in @angles.product(@axes)
156
+ q = ::UnitQuaternion.fromAngleAxis(angle, axis)
157
+ a, ax = q.getAngleAxis()
158
+ q2 = ::UnitQuaternion.fromAngleAxis(a, ax)
159
+ assert(areEqualMatrices(q.getRotationMatrix(), q2.getRotationMatrix(),
160
+ 1e-14))
161
+ end
162
+ end
163
+
164
+ def test_multiply_inverse
165
+ axis1 = Vector[1, 2, 3]
166
+ angle1 = 1.123
167
+ q1 = ::UnitQuaternion.fromAngleAxis(angle1, axis1)
168
+
169
+ axis2 = Vector[1, 2, 3]
170
+ angle2 = -1.123
171
+ q2 = ::UnitQuaternion.fromAngleAxis(angle2, axis2)
172
+
173
+ q3 = q1 * q2
174
+ beta0, beta_s = q3.get()
175
+ assert_in_delta(1, beta0, 1e-15)
176
+ assert_in_delta(0, beta_s.norm(), 1e-15)
177
+
178
+ for q in @quats
179
+ q_inv = q.inverse()
180
+ result = q * q_inv
181
+ beta0, beta_s = result.get()
182
+ assert_in_delta(1, beta0, 1e-15)
183
+ assert_in_delta(0, beta_s.norm(), 1e-14)
184
+ end
185
+ end
186
+
187
+ def test_multiply
188
+ axis1 = Vector[3, 1, 6]
189
+ angle1 = 1.32
190
+ q1 = ::UnitQuaternion.fromAngleAxis(angle1, axis1)
191
+
192
+ axis2 = Vector[1, 1, 1]
193
+ angle2 = Math::PI/4
194
+ q2 = ::UnitQuaternion.fromAngleAxis(angle2, axis2)
195
+
196
+ q_result = q2*q1
197
+ q_result_mat = q_result.getRotationMatrix()
198
+
199
+ mat_result = q2.getRotationMatrix() * q1.getRotationMatrix()
200
+
201
+ for i in 0..2
202
+ assert_in_delta((q_result_mat.row(i) - mat_result.row(i)).norm(),
203
+ 0, 1e-15)
204
+ end
205
+
206
+ for q1, q2 in @quats.product(@quats)
207
+ q_result = q2 * q1
208
+ q_result_mat = q_result.getRotationMatrix()
209
+ mat_result = q2.getRotationMatrix() * q1.getRotationMatrix()
210
+
211
+ for i in 0..2
212
+ assert_in_delta((q_result_mat.row(i) - mat_result.row(i)).norm(),
213
+ 0, 1e-14)
214
+ end
215
+ end
216
+ end
217
+
218
+ def test_transform
219
+ axis = Vector[1,1,1]
220
+ angle = 2*Math::PI/3
221
+ q1 = ::UnitQuaternion.fromAngleAxis(angle, axis)
222
+
223
+ v = Vector[1,0,0]
224
+ v_rot = q1.transform(v)
225
+ expected = Vector[0,1,0]
226
+
227
+ assert_in_delta((v_rot - expected).norm(), 0, 1e-15)
228
+
229
+ for q, v in @quats.product(@axes)
230
+ v_rot = q.transform(v)
231
+ v_expected = q.getRotationMatrix() * v
232
+ assert_in_delta((v_rot - v_expected).norm(), 0, 1e-15)
233
+ end
234
+ end
235
+
236
+ def test_inverse
237
+ angles = [ 0, Math::PI/4, 0.1234, Math::PI/2, 2 ]
238
+ axes = [ Vector[1,1,1], Vector[1,2,3], Vector[0,0,1] ]
239
+ for angle, axis in (angles + @angles).product(axes + @axes)
240
+ q = ::UnitQuaternion.fromAngleAxis(angle, axis)
241
+ q_inv = q.inverse()
242
+
243
+ assert(isIdentityMatrix( q.getRotationMatrix() *
244
+ q_inv.getRotationMatrix(),
245
+ 1e-15 )
246
+ )
247
+
248
+ result = q * q_inv
249
+ beta0, beta_s = result.get()
250
+ assert_in_delta(beta0, 1, 1e-15)
251
+ assert_in_delta((beta_s - Vector[0,0,0]).norm(), 0, 1e-15)
252
+ end
253
+ end
254
+
255
+ def test_Euler
256
+ # If we generate a quaternion using Euler angles and then ask for
257
+ # the Euler angles from that quaternion, we may get a different
258
+ # answer, since the Euler angles are not unique. However, if we
259
+ # ask for the Euler angles from the first quaternion, then
260
+ # generate a second quaternion using those same Euler angles, the
261
+ # quaternions should be equal to each other.
262
+
263
+ @angles.product(@angles, @angles) do | theta1, theta2, theta3 |
264
+ q = UnitQuaternion.fromEuler(theta1, theta2, theta3, 'xyz')
265
+ @euler.each do |e|
266
+ q2 = UnitQuaternion.fromEuler(*q.getEuler(e), e)
267
+ tol = 1e-7
268
+ assert(areEqualMatrices(q.getRotationMatrix(),
269
+ q2.getRotationMatrix(), tol))
270
+ end
271
+ end
272
+
273
+ q = UnitQuaternion.fromEuler(2 * Math::PI/2, 2 * Math::PI/2,
274
+ 2 * Math::PI/2, 'xyz')
275
+ assert_in_delta((q - UnitQuaternion.new(-1,0,0,0)).norm(), 0, 1e-15)
276
+
277
+ assert_raise(ArgumentError) { q.getEuler('xy') }
278
+ assert_raise(ArgumentError) { q.getEuler('xyzx') }
279
+ assert_raise(ArgumentError) { q.getEuler('xYz') }
280
+ assert_raise(ArgumentError) { q.getEuler('xxy') }
281
+ assert_raise(ArgumentError) { q.getEuler('yyz') }
282
+ assert_raise(ArgumentError) { q.getEuler('xzz') }
283
+ assert_raise(ArgumentError) { q.getEuler('xzb') }
284
+
285
+ assert_raise(ArgumentError) { UnitQuaternion.fromEuler(0, 0, 0, 'xy') }
286
+ assert_raise(ArgumentError) { UnitQuaternion.fromEuler(1, 1, 1, 'x') }
287
+ assert_raise(ArgumentError) { UnitQuaternion.fromEuler(0, 0, 0, 'xyzx') }
288
+ assert_raise(ArgumentError) { UnitQuaternion.fromEuler(0, 0, 0, 'xya') }
289
+ assert_raise(ArgumentError) { UnitQuaternion.fromEuler(0, 0, 0, 'Xyz') }
290
+ end
291
+
292
+ def test_rotationMatrix
293
+ tol = 1e-7
294
+ @angles.product(@angles, @angles) do | theta1, theta2, theta3 |
295
+ q = ::UnitQuaternion.fromEuler(theta1, theta2, theta3, 'XYZ')
296
+
297
+ q_from = ::UnitQuaternion.fromRotationMatrix(q.getRotationMatrix())
298
+ q_set = ::UnitQuaternion.new()
299
+ q_set.setRotationMatrix(q.getRotationMatrix())
300
+
301
+ assert(areEqualMatrices(q.getRotationMatrix(),
302
+ q_from.getRotationMatrix(), tol))
303
+ assert(areEqualMatrices(q.getRotationMatrix(),
304
+ q_set.getRotationMatrix(), tol))
305
+ end
306
+
307
+ assert_raise(ArgumentError) do
308
+ UnitQuaternion.fromRotationMatrix(Matrix[ [1, 1, 0],
309
+ [0, 1, 0],
310
+ [0, 0, 1] ])
311
+ end
312
+ assert_raise(ArgumentError) do
313
+ UnitQuaternion.fromRotationMatrix(Matrix[ [1, 1e-8, 0],
314
+ [0, 1, 0],
315
+ [0, 0, 1] ])
316
+ end
317
+ assert_raise(ArgumentError) do
318
+ UnitQuaternion.fromRotationMatrix(Matrix[ [0, 1, 0],
319
+ [0, 1, 0],
320
+ [0, 0, 1] ])
321
+ end
322
+ assert_raise(ArgumentError) do
323
+ UnitQuaternion.fromRotationMatrix(Matrix[ [1, 0, 0],
324
+ [0, 1, 0] ])
325
+ end
326
+ assert_raise(ArgumentError) do
327
+ UnitQuaternion.fromRotationMatrix(Matrix[ [1, 0, 0, 0],
328
+ [0, 1, 0, 0],
329
+ [0, 0, 1, 0],
330
+ [0, 0, 0, 1] ])
331
+ end
332
+ assert_raise(ArgumentError) do
333
+ UnitQuaternion.fromRotationMatrix(Matrix[ [1, 0],
334
+ [0, 1] ])
335
+ end
336
+ end
337
+ end
@@ -0,0 +1,25 @@
1
+ # coding: utf-8
2
+ lib = File.expand_path('../lib', __FILE__)
3
+ $LOAD_PATH.unshift(lib) unless $LOAD_PATH.include?(lib)
4
+ require 'unit_quaternion/version'
5
+
6
+ Gem::Specification.new do |spec|
7
+ spec.name = "unit_quaternion"
8
+ spec.version = Quaternion::VERSION
9
+ spec.authors = ["Cory Crean"]
10
+ spec.email = ["cory.crean@gmail.com"]
11
+ spec.description = %q{Provides a general Quaternion class, and UnitQuaternion class to represent rotations}
12
+ spec.summary = spec.description
13
+ spec.homepage = ""
14
+ spec.license = "BSD"
15
+
16
+ spec.files = `git ls-files`.split($/)
17
+ spec.executables = spec.files.grep(%r{^bin/}) { |f| File.basename(f) }
18
+ spec.test_files = spec.files.grep(%r{^(tests|spec|features)/})
19
+ spec.require_paths = ["lib"]
20
+
21
+ spec.add_development_dependency "bundler", "~> 1.3"
22
+ spec.add_development_dependency "rake"
23
+ spec.add_development_dependency "simplecov", "~> 0.11.2"
24
+ spec.add_development_dependency "simplecov-html", "~> 0.10.0"
25
+ end
metadata ADDED
@@ -0,0 +1,127 @@
1
+ --- !ruby/object:Gem::Specification
2
+ name: unit_quaternion
3
+ version: !ruby/object:Gem::Version
4
+ version: 0.0.2
5
+ prerelease:
6
+ platform: ruby
7
+ authors:
8
+ - Cory Crean
9
+ autorequire:
10
+ bindir: bin
11
+ cert_chain: []
12
+ date: 2016-07-02 00:00:00.000000000 Z
13
+ dependencies:
14
+ - !ruby/object:Gem::Dependency
15
+ name: bundler
16
+ requirement: !ruby/object:Gem::Requirement
17
+ none: false
18
+ requirements:
19
+ - - ~>
20
+ - !ruby/object:Gem::Version
21
+ version: '1.3'
22
+ type: :development
23
+ prerelease: false
24
+ version_requirements: !ruby/object:Gem::Requirement
25
+ none: false
26
+ requirements:
27
+ - - ~>
28
+ - !ruby/object:Gem::Version
29
+ version: '1.3'
30
+ - !ruby/object:Gem::Dependency
31
+ name: rake
32
+ requirement: !ruby/object:Gem::Requirement
33
+ none: false
34
+ requirements:
35
+ - - ! '>='
36
+ - !ruby/object:Gem::Version
37
+ version: '0'
38
+ type: :development
39
+ prerelease: false
40
+ version_requirements: !ruby/object:Gem::Requirement
41
+ none: false
42
+ requirements:
43
+ - - ! '>='
44
+ - !ruby/object:Gem::Version
45
+ version: '0'
46
+ - !ruby/object:Gem::Dependency
47
+ name: simplecov
48
+ requirement: !ruby/object:Gem::Requirement
49
+ none: false
50
+ requirements:
51
+ - - ~>
52
+ - !ruby/object:Gem::Version
53
+ version: 0.11.2
54
+ type: :development
55
+ prerelease: false
56
+ version_requirements: !ruby/object:Gem::Requirement
57
+ none: false
58
+ requirements:
59
+ - - ~>
60
+ - !ruby/object:Gem::Version
61
+ version: 0.11.2
62
+ - !ruby/object:Gem::Dependency
63
+ name: simplecov-html
64
+ requirement: !ruby/object:Gem::Requirement
65
+ none: false
66
+ requirements:
67
+ - - ~>
68
+ - !ruby/object:Gem::Version
69
+ version: 0.10.0
70
+ type: :development
71
+ prerelease: false
72
+ version_requirements: !ruby/object:Gem::Requirement
73
+ none: false
74
+ requirements:
75
+ - - ~>
76
+ - !ruby/object:Gem::Version
77
+ version: 0.10.0
78
+ description: Provides a general Quaternion class, and UnitQuaternion class to represent
79
+ rotations
80
+ email:
81
+ - cory.crean@gmail.com
82
+ executables: []
83
+ extensions: []
84
+ extra_rdoc_files: []
85
+ files:
86
+ - .gitignore
87
+ - Gemfile
88
+ - LICENSE.txt
89
+ - README.md
90
+ - Rakefile
91
+ - lib/quaternion.rb
92
+ - lib/unit_quaternion.rb
93
+ - lib/unit_quaternion/version.rb
94
+ - tests/tests.rb
95
+ - tests/tests_Quaternion.rb
96
+ - tests/tests_UnitQuaternion.rb
97
+ - unit_quaternion.gemspec
98
+ homepage: ''
99
+ licenses:
100
+ - BSD
101
+ post_install_message:
102
+ rdoc_options: []
103
+ require_paths:
104
+ - lib
105
+ required_ruby_version: !ruby/object:Gem::Requirement
106
+ none: false
107
+ requirements:
108
+ - - ! '>='
109
+ - !ruby/object:Gem::Version
110
+ version: '0'
111
+ required_rubygems_version: !ruby/object:Gem::Requirement
112
+ none: false
113
+ requirements:
114
+ - - ! '>='
115
+ - !ruby/object:Gem::Version
116
+ version: '0'
117
+ requirements: []
118
+ rubyforge_project:
119
+ rubygems_version: 1.8.23
120
+ signing_key:
121
+ specification_version: 3
122
+ summary: Provides a general Quaternion class, and UnitQuaternion class to represent
123
+ rotations
124
+ test_files:
125
+ - tests/tests.rb
126
+ - tests/tests_Quaternion.rb
127
+ - tests/tests_UnitQuaternion.rb