lib/nmatrix/homogeneous.rb in nmatrix-0.1.0.rc5 vs lib/nmatrix/homogeneous.rb in nmatrix-0.1.0

@@ -138,6 +138,104 @@
       end
       n[0..2,3] = xyz
       n
     end
   end
+
+  #
+  # call-seq:
+  #     quaternion -> NMatrix
+  #
+  # Find the quaternion for a 3D rotation matrix.
+  #
+  # Code borrowed from: http://courses.cms.caltech.edu/cs171/quatut.pdf
+  #
+  # * *Returns* :
+  #   - A length-4 NMatrix representing the corresponding quaternion.
+  #
+  # Examples:
+  #
+  #    n.quaternion # => [1, 0, 0, 0]
+  #
+  def quaternion
+    raise(ShapeError, "Expected square matrix") if self.shape[0] != self.shape[1]
+    raise(ShapeError, "Expected 3x3 rotation (or 4x4 homogeneous) matrix") if self.shape[0] > 4 || self.shape[0] < 3
+
+    q = NMatrix.new([4], dtype: self.dtype == :float32 ? :float32: :float64)
+    rotation_trace = self[0,0] + self[1,1] + self[2,2]
+    if rotation_trace >= 0
+      self_w = self.shape[0] == 4 ? self[3,3] : 1.0
+      root_of_homogeneous_trace = Math.sqrt(rotation_trace + self_w)
+      q[0] = root_of_homogeneous_trace * 0.5
+      s = 0.5 / root_of_homogeneous_trace
+      q[1] = (self[2,1] - self[1,2]) * s
+      q[2] = (self[0,2] - self[2,0]) * s
+      q[3] = (self[1,0] - self[0,1]) * s
+    else
+      h = 0
+      h = 1 if self[1,1] > self[0,0]
+      h = 2 if self[2,2] > self[h,h]
+
+      case_macro = Proc.new do |i,j,k,ii,jj,kk|
+        qq = NMatrix.new([4], dtype: :float64)
+        self_w = self.shape[0] == 4 ? self[3,3] : 1.0
+        s = Math.sqrt( (self[ii,ii] - (self[jj,jj] + self[kk,kk])) + self_w)
+        qq[i] = s*0.5
+        s = 0.5 / s
+        qq[j] = (self[ii,jj] + self[jj,ii]) * s
+        qq[k] = (self[kk,ii] + self[ii,kk]) * s
+        qq[0] = (self[kk,jj] - self[jj,kk]) * s
+        qq
+      end
+
+      case h
+      when 0
+        q = case_macro.call(1,2,3, 0,1,2)
+      when 1
+        q = case_macro.call(2,3,1, 1,2,0)
+      when 2
+        q = case_macro.call(3,1,2, 2,0,1)
+      end
+
+      self_w = self.shape[0] == 4 ? self[3,3] : 1.0
+      if self_w != 1
+        s = 1.0 / Math.sqrt(self_w)
+        q[0] *= s
+        q[1] *= s
+        q[2] *= s
+        q[3] *= s
+      end
+    end
+
+    q
+  end
+
+  #
+  # call-seq:
+  #     angle_vector -> [angle, about_vector]
+  #
+  # Find the angle vector for a quaternion. Assumes the quaternion has unit length.
+  #
+  # Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
+  #
+  # * *Returns* :
+  #   - An angle (in radians) describing the rotation about the +about_vector+.
+  #   - A length-3 NMatrix representing the corresponding quaternion.
+  #
+  # Examples:
+  #
+  #    q.angle_vector # => [1, 0, 0, 0]
+  #
+  def angle_vector
+    raise(ShapeError, "Expected length-4 vector or matrix (quaternion)") if self.shape[0] != 4
+    raise("Expected unit quaternion") if self[0] > 1
+
+    xyz = NMatrix.new([3], dtype: self.dtype)
+
+    angle = 2 * Math.acos(self[0])
+    s = Math.sqrt(1.0 - self[0]*self[0])
+
+    xyz[0..2] = self[1..3]
+    xyz /= s if s >= 0.001 # avoid divide by zero
+    return [angle, xyz]
+  end
 end
\ No newline at end of file