Class: UnitQuaternion

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

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
39
40
 
# File 'lib/unit_quaternion.rb', line 34

def self.fromAngleAxis(angle, axis)
  # intializes a quaternion from the angle-axis representation of a
  # rotation
  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).



49
50
51
52
53
 
# File 'lib/unit_quaternion.rb', line 49

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.



59
60
61
62
63
 
# File 'lib/unit_quaternion.rb', line 59

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



109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
 
# File 'lib/unit_quaternion.rb', line 109

def getAngleAxis
  # return the angle-axis representation of the rotation contained in
  # this quaternion
  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



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
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
 
# File 'lib/unit_quaternion.rb', line 168

def getEuler(axes)
  # Returns the Euler angles about the specified axes.  The axes should
  # be specified as a string, and can be any permutation (with
  # replacement) of 'X', 'Y', and 'Z', as long as no letter is adjacent
  # to itself (for example, 'XYX' is valid, but 'XXY' is not).
  # If the axes are uppercase, this function returns the angles about
  # the global axes.  If they are lowercase, this function returns the
  # angles about the body-fixed 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.



265
266
267
268
269
270
271
272
273
274
275
276
 
# File 'lib/unit_quaternion.rb', line 265

def getRotationMatrix
  # returns the rotation matrix corresponding to this quaternion
  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



288
289
290
291
292
 
# File 'lib/unit_quaternion.rb', line 288

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



73
74
75
76
77
 
# File 'lib/unit_quaternion.rb', line 73

def set(w, x, y, z)
  # sets the values of the quaternion
  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)



86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
 
# File 'lib/unit_quaternion.rb', line 86

def setAngleAxis(angle, axis)
  # sets the quaternion based on the angle-axis representation of a
  # rotation
  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).



132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
 
# File 'lib/unit_quaternion.rb', line 132

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.



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

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.



283
284
285
 
# File 'lib/unit_quaternion.rb', line 283

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