lib/nmatrix/homogeneous.rb
#--
# = NMatrix
#
# A linear algebra library for scientific computation in Ruby.
# NMatrix is part of SciRuby.
#
# NMatrix was originally inspired by and derived from NArray, by
# Masahiro Tanaka: http://narray.rubyforge.org
#
# == Copyright Information
#
# SciRuby is Copyright (c) 2010 - 2014, Ruby Science Foundation
# NMatrix is Copyright (c) 2012 - 2014, John Woods and the Ruby Science Foundation
#
# Please see LICENSE.txt for additional copyright notices.
#
# == Contributing
#
# By contributing source code to SciRuby, you agree to be bound by
# our Contributor Agreement:
#
# * https://github.com/SciRuby/sciruby/wiki/Contributor-Agreement
#
# == homogeneous.rb
#
# This file contains optional shortcuts for generating homogeneous
# transformations.
#
#++
class NMatrix
class << self
#
# call-seq:
# x_rotation(angle_in_radians) -> NMatrix
# x_rotation(angle_in_radians, dtype: dtype) -> NMatrix
# y_rotation(angle_in_radians) -> NMatrix
# y_rotation(angle_in_radians, dtype: dtype) -> NMatrix
# z_rotation(angle_in_radians) -> NMatrix
# z_rotation(angle_in_radians, dtype: dtype) -> NMatrix
#
# Generate a 4x4 homogeneous transformation matrix representing a rotation
# about the x, y, or z axis respectively.
#
# * *Arguments* :
# - +angle_in_radians+ -> The angle of rotation in radians.
# - +dtype+ -> (optional) Default is +:float64+
# * *Returns* :
# - A homogeneous transformation matrix consisting of a single rotation.
#
# Examples:
#
# NMatrix.x_rotation(Math::PI.quo(6)) # =>
# 1.0 0.0 0.0 0.0
# 0.0 0.866025 -0.499999 0.0
# 0.0 0.499999 0.866025 0.0
# 0.0 0.0 0.0 1.0
#
#
# NMatrix.x_rotation(Math::PI.quo(6), dtype: :float32) # =>
# 1.0 0.0 0.0 0.0
# 0.0 0.866025 -0.5 0.0
# 0.0 0.5 0.866025 0.0
# 0.0 0.0 0.0 1.0
#
def x_rotation angle_in_radians, opts={}
c = Math.cos(angle_in_radians)
s = Math.sin(angle_in_radians)
NMatrix.new(4, [1.0, 0.0, 0.0, 0.0,
0.0, c, -s, 0.0,
0.0, s, c, 0.0,
0.0, 0.0, 0.0, 1.0], {dtype: :float64}.merge(opts))
end
def y_rotation angle_in_radians, opts={}
c = Math.cos(angle_in_radians)
s = Math.sin(angle_in_radians)
NMatrix.new(4, [ c, 0.0, s, 0.0,
0.0, 1.0, 0.0, 0.0,
-s, 0.0, c, 0.0,
0.0, 0.0, 0.0, 1.0], {dtype: :float64}.merge(opts))
end
def z_rotation angle_in_radians, opts={}
c = Math.cos(angle_in_radians)
s = Math.sin(angle_in_radians)
NMatrix.new(4, [ c, -s, 0.0, 0.0,
s, c, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0], {dtype: :float64}.merge(opts))
end
#
# call-seq:
# translation(x, y, z) -> NMatrix
# translation([x,y,z]) -> NMatrix
# translation(translation_matrix) -> NMatrix
# translation(translation_matrix) -> NMatrix
# translation(translation, dtype: dtype) -> NMatrix
# translation(x, y, z, dtype: dtype) -> NMatrix
#
# Generate a 4x4 homogeneous transformation matrix representing a translation.
#
# * *Returns* :
# - A homogeneous transformation matrix consisting of a translation.
#
# Examples:
#
# NMatrix.translation(4.0,5.0,6.0) # =>
# 1.0 0.0 0.0 4.0
# 0.0 1.0 0.0 5.0
# 0.0 0.0 1.0 6.0
# 0.0 0.0 0.0 1.0
#
# NMatrix.translation(4.0,5.0,6.0, dtype: :int64) # =>
# 1 0 0 4
# 0 1 0 5
# 0 0 1 6
# 0 0 0 1
# NMatrix.translation(4,5,6) # =>
# 1 0 0 4
# 0 1 0 5
# 0 0 1 6
# 0 0 0 1
#
def translation *args
xyz = args.shift if args.first.is_a?(NMatrix) || args.first.is_a?(Array)
default_dtype = xyz.respond_to?(:dtype) ? xyz.dtype : NMatrix.guess_dtype(xyz)
opts = {dtype: default_dtype}
opts = opts.merge(args.pop) if args.size > 0 && args.last.is_a?(Hash)
xyz ||= args
n = if args.size > 0
NMatrix.eye(4, opts)
else
NMatrix.eye(4, opts)
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