Class: UnitQuaternion

Inherits:
Quaternion show all
Defined in:
lib/unit_quaternion.rb,
lib/unit_quaternion/version.rb

Constant Summary collapse

VERSION = 
"0.0.4"

Class Method Summary collapse

Instance Method Summary collapse

Methods inherited from Quaternion

#*, #+, #-, #-@, #/, #==, #coerce, #conjugate, #get, #norm, #normalized, #to_s

Constructor Details

#initialize(*args) ⇒ UnitQuaternion

Creates a new UnitQuaternion from 4 values. If the resulting quaternion does not have magnitude 1, it will be normalized. If no arguments are provided, creates the quaternion (1, 0, 0, 0).



17
18
19
20
21
22
23
24
25
 
# File 'lib/unit_quaternion.rb', line 17

def initialize(*args)
  if args.length() == 4
    set(*args)
  elsif args.length() == 0
    super(1, 0, 0, 0)
  else
    raise(ArgumentError, "wrong number of arguments (must be 0 or 4)")
  end
end

Class Method Details

.fromAngleAxis(angle, axis) ⇒ Object

Initializes a quaternion from the angle-axis representation of a rotation. The sense of the rotation is determined according to the right-hand rule.

Params:

angle

A scalar representing the angle of the rotation (in radians)

axis

A vector representing the axis of rotation (need not be a unit vector)



34
35
36
37
38
 
# File 'lib/unit_quaternion.rb', line 34

def self.fromAngleAxis(angle, axis)
  q = UnitQuaternion.new()
  q.setAngleAxis(angle, axis)
  return q
end

.fromEuler(theta1, theta2, theta3, axes) ⇒ Object

Initializes a quaternion from a set of 3 Euler angles.

Params:

theta1

The angle of rotation about the first axis

theta2

The angle of rotation about the second axis

theta3

The angle of rotation about the third axis

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).



47
48
49
50
51
 
# File 'lib/unit_quaternion.rb', line 47

def self.fromEuler(theta1, theta2, theta3, axes)
  q = UnitQuaternion.new()
  q.setEuler(theta1, theta2, theta3, axes)
  return q
end

.fromRotationMatrix(mat) ⇒ Object

Initializes a quaternion from a rotation matrix.

Params:

mat

A 3x3 orthonormal matrix.



57
58
59
60
61
 
# File 'lib/unit_quaternion.rb', line 57

def self.fromRotationMatrix(mat)
  q = UnitQuaternion.new()
  q.setRotationMatrix(mat)
  return q
end

Instance Method Details

#getAngleAxis ⇒ Object

Returns the angle-axis representation of the rotation represented by the quaternion.

Returns:

angle

A scalar representing the angle of rotation (in radians)

axis

A unit vector representing the axis of rotation



104
105
106
107
108
109
110
111
112
113
114
115
116
 
# File 'lib/unit_quaternion.rb', line 104

def getAngleAxis
  angle = 2*Math.acos(@beta0)
  
  # if sin(theta/2) = 0, then theta = 2*n*PI, where n is any integer,
  # which means that the object has performed a complete rotation, and
  # any axis will do
  if Math.sin(angle/2).abs() < 1e-15
    axis = Vector[1, 0, 0]
  else
    axis = @beta_s / Math.sin(angle/2)
  end
  return angle, axis
end

#getEuler(axes) ⇒ Object

Returns the Euler angles corresponding to this quaternion.

Params:

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).

Returns:

theta1

The angle of rotation about the first axis

theta2

The angle of rotation about the second axis

theta3

The angle of rotation about the third axis



161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
 
# File 'lib/unit_quaternion.rb', line 161

def getEuler(axes)
  # This method implements Shoemake's algorithm for finding the Euler
  # angles from a rotation matrix, found in Graphics Gems IV (pg. 222).

  if axes.length() != 3
    raise(ArgumentError, "Exactly 3 axes must be specified in order to " +
          "calculate the Euler angles")
  end

  if axes == axes.upcase()
    # get angles about global axes
    static = true
  elsif axes == axes.downcase()
    # get angles about body-fixes axes
    static = false
    axes = axes.reverse()
  else
    raise(ArgumentError, "Axes must either be all uppercase or all " +
          "lowercase")
  end

  axes = axes.upcase()
  if axes[0] == axes[2]
    same = true
  end

  if not ('XYZ'.include?(axes[0]) and 'XYZ'.include?(axes[1]) and
          'XYZ'.include?(axes[2]) )
    raise(ArgumentError, 'Axes can only be X/x, Y/y, or Z/z')
  end

  if axes.include?('XX') or axes.include?('YY') or
      axes.include?('ZZ')
    raise(ArgumentError, "Cannot rotate about the same axis twice in " +
          "succession")
  end

  # true if the axes specify a right-handed coordinate system, false
  # otherwise
  rh = isRightHanded(axes.upcase())

  p_mat_rows = Array.new()
  unused = [ 'X', 'Y', 'Z' ]
  axes[0..1].each_char() do |a|
    if a == 'X'
      p_mat_rows << getUnitVector(a)
    elsif a == 'Y'
      p_mat_rows << getUnitVector(a)
    elsif a == 'Z'
      p_mat_rows << getUnitVector(a)
    end
    unused.delete(a)
  end
  p_mat_rows << getUnitVector(unused[0])
  
  p_mat = Matrix.rows(p_mat_rows)
  rot_mat = p_mat * getRotationMatrix() * p_mat.transpose()

  theta1, theta2, theta3 = parseMatrix(rot_mat, same)

  if not static
    theta1, theta3 = theta3, theta1
  end

  if not rh
    theta1, theta2, theta3 = -theta1, -theta2, -theta3
  end

  return theta1, theta2, theta3
end

#getRotationMatrix ⇒ Object

Returns the rotation matrix corresponding to this quaternion.



250
251
252
253
254
255
256
257
258
259
260
 
# File 'lib/unit_quaternion.rb', line 250

def getRotationMatrix
  return Matrix[ [ @beta0**2 + @beta_s[0]**2 - @beta_s[1]**2 - @beta_s[2]**2,
                   2*(@beta_s[0]*@beta_s[1] - @beta0*@beta_s[2]),
                   2*(@beta_s[0]*@beta_s[2] + @beta0*@beta_s[1]) ],
                 [ 2*(@beta_s[0]*@beta_s[1] + @beta0*@beta_s[2]),
                   @beta0**2 - @beta_s[0]**2 + @beta_s[1]**2 - @beta_s[2]**2,
                   2*(@beta_s[1]*@beta_s[2] - @beta0*@beta_s[0]) ],
                 [ 2*(@beta_s[0]*@beta_s[2] - @beta0*@beta_s[1]),
                   2*(@beta0*@beta_s[0] + @beta_s[1]*@beta_s[2]),
                   @beta0**2 - @beta_s[0]**2 - @beta_s[1]**2 + @beta_s[2]**2 ] ]
end

#inverse ⇒ Object

Returns the inverse of the quaternion.



272
273
274
275
276
 
# File 'lib/unit_quaternion.rb', line 272

def inverse
  result = UnitQuaternion.new
  result.set(@beta0, *(-1*@beta_s))
  return result
end

#set(w, x, y, z) ⇒ Object

Set the quaternion’s values. If the 4 arguments do not form an unit quaternion, the resulting quaternion is normalized.

Params:

w

the real part of the quaterion

x

the i-component

y

the j-component

z

the k-component



71
72
73
74
 
# File 'lib/unit_quaternion.rb', line 71

def set(w, x, y, z)
  super(w, x, y, z)
  @beta0, @beta_s = normalized().get()
end

#setAngleAxis(angle, axis) ⇒ Object

Sets the values of the quaternion from the angle-axis representation of a rotation. The sense of the rotation is determined according to the right-hand rule.

Params:

angle

A scalar representing the angle of the rotation (in radians)

axis

A vector representing the axis of rotation (need not be a unit vector)



83
84
85
86
87
88
89
90
91
92
93
94
95
96
 
# File 'lib/unit_quaternion.rb', line 83

def setAngleAxis(angle, axis)
  if axis == Vector[0,0,0]
    raise(ArgumentError, "Axis must not be the zero vector")
  end
  if axis.size() != 3
    raise(ArgumentError, "Axis must be a 3-dimensional vector")
  end
  axis = axis.normalize()
  @beta0 = Math.cos(angle / 2.0)
  beta1 = axis[0] * Math.sin(angle / 2.0)
  beta2 = axis[1] * Math.sin(angle / 2.0)
  beta3 = axis[2] * Math.sin(angle / 2.0)
  @beta_s = Vector[beta1, beta2, beta3]
end

#setEuler(theta1, theta2, theta3, axes) ⇒ Object

Sets the values of the quaternion from a set of 3 Euler angles.

Params:

theta1

The angle of rotation about the first axis

theta2

The angle of rotation about the second axis

theta3

The angle of rotation about the third axis

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).



125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
 
# File 'lib/unit_quaternion.rb', line 125

def setEuler(theta1, theta2, theta3, axes)
  if axes.length() != 3
    raise(ArgumentError, "Must specify exactly 3 axes")
  end
  quats = Array.new(3)
  theta = [theta1, theta2, theta3]
  for i in 0..2
    if axes.upcase()[i] == 'X'
      quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[1, 0, 0])
    elsif axes.upcase()[i] == 'Y'
      quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[0, 1, 0])
    elsif axes.upcase()[i] == 'Z'
      quats[i] = UnitQuaternion.fromAngleAxis(theta[i], Vector[0, 0, 1])
    else
      raise(ArgumentError, "Axes can only be X/x, Y/y, or Z/z")
    end
  end
  if axes == axes.upcase()
    @beta0, @beta_s = (quats[2] * quats[1] * quats[0]).get()
  elsif axes == axes.downcase()
    @beta0, @beta_s = (quats[0] * quats[1] * quats[2]).get()
  else
    raise(ArgumentError, "Axes must be either all uppercase or all " +
          "lowercase")
  end
end

#setRotationMatrix(mat) ⇒ Object

Sets the values of the quaternion from a rotation matrix.

Params:

mat

A 3x3 orthonormal matrix.



236
237
238
239
240
241
242
243
244
245
246
247
 
# File 'lib/unit_quaternion.rb', line 236

def setRotationMatrix(mat)
  if mat.row_size() != 3 or mat.column_size() != 3
    raise(ArgumentError, "Rotation matrix must be 3x3")
  end
  tol = 1e-15
  if not isOrthonormalMatrix(mat, tol)
    raise(ArgumentError, "Matrix is not orthonormal (to within " +
          tol.to_s(), ")")
  end
  theta1, theta2, theta3 = parseMatrix(mat, false)
  setEuler(theta1, theta2, theta3, 'XYZ')
end

#transform(vec) ⇒ Object

Transforms a vector by applying to it the rotation represented by this quaternion, and returns the result.

Params:

vec

A 3-D vector in the unrotated frame.



267
268
269
 
# File 'lib/unit_quaternion.rb', line 267

def transform(vec)
  return getRotationMatrix() * vec
end