unit_quaternion 0.0.2
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- data/.gitignore +6 -0
- data/Gemfile +4 -0
- data/LICENSE.txt +25 -0
- data/README.md +103 -0
- data/Rakefile +5 -0
- data/lib/quaternion.rb +127 -0
- data/lib/unit_quaternion/version.rb +3 -0
- data/lib/unit_quaternion.rb +413 -0
- data/tests/tests.rb +12 -0
- data/tests/tests_Quaternion.rb +157 -0
- data/tests/tests_UnitQuaternion.rb +337 -0
- data/unit_quaternion.gemspec +25 -0
- metadata +127 -0
data/.gitignore
ADDED
data/Gemfile
ADDED
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
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,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
|