homogeneous_transformation 0.0.1

data/.gitignore

@@ -0,0 +1,24 @@
+*.gem
+*.rbc
+.bundle
+.config
+.yardoc
+Gemfile.lock
+InstalledFiles
+_yardoc
+coverage
+doc/
+lib/bundler/man
+pkg
+rdoc
+spec/reports
+test/tmp
+test/version_tmp
+tmp
+
+log.org
+tester.rb
+*~
+coverage/*
+doc/*
+other/*

data/Gemfile

@@ -0,0 +1,4 @@
+source 'https://rubygems.org'
+
+# Specify your gem's dependencies in homogeneous_transformation.gemspec
+gemspec

data/LICENSE.txt

@@ -0,0 +1,25 @@
+Copyright (c) 2016, Cory Crean
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * The name(s) of the author(s) may not be used to endorse or promote
+      products derived from this software without specific prior written
+      permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CORY CREAN BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

data/README.md

@@ -0,0 +1,197 @@
+# HomogeneousTransformation
+
+Homogeneous transformations are common in robotics, where each of the
+bodies that make up the robot has a coordinate frame associated with
+it.  In general, the coordinate frames for two different bodies will
+have different orientations and different locations in space.  If you
+have a vector that describes the location of a point relative to one
+reference frame, the question arises of how to find a vector that
+describes the location of the same point relative to a different
+reference frame.  A homogeneous transformation provides a
+straightforward way to perform the conversion.  See the usage
+instructions below for more details.
+
+## Installation
+
+Add this line to your application's Gemfile:
+
+    gem 'homogeneous_transformation'
+
+And then execute:
+
+    $ bundle
+
+Or install it yourself as:
+
+    $ gem install homogeneous_transformation
+
+## Usage
+
+To use the HomogeneousTransformation class in your program, include
+the following line:
+
+```
+require 'homogeneous_transformation'
+```
+
+The following typedef has bee provided for your convenience:
+
+```
+HomTran = HomogeneousTransformation
+```
+
+We will use it throughout the remainder of this readme for the sake of
+brevity.
+
+In order to initialize a HomogeneousTransformation, you need to create
+a UnitQuaternion object that represents the HomTran's orientation, and
+you need to provide a vector that gives the HomTran's location in
+space.  The [unit_quaternion
+gem](https://rubygems.org/gems/unit_quaternion) is required
+automatically when you require the homogeneous_transformation gem.
+
+```
+q = UnitQuaternion.fromAngleAxis(Math::PI/2, Vector[1,1,1])
+t = Vector[1,2,3]
+frame = HomTran.new(q, t)
+=> #<HomogeneousTransformation:0x00000002595a68 @q=(0.7071067811865476, Vector[0.408248290463863, 0.408248290463863, 0.408248290463863]), @t=Vector[1, 2, 3]>
+```
+
+You can recover the orientation or translation for a given HomTran
+object using the following methods:
+
+```
+frame.getQuaternion()
+=> (0.7071067811865476, Vector[0.408248290463863, 0.408248290463863, 0.408248290463863])
+
+frame.getTranslation()
+=> Vector[1, 2, 3]
+```
+
+You can get the 4x4 matrix representation of the HomTran as follows:
+
+```
+frame.getMatrix()
+=> Matrix[[0.3333333333333335, -0.24401693585629253, 0.9106836025229592, 1], [0.9106836025229592, 0.3333333333333335, -0.24401693585629253, 2], [-0.24401693585629253, 0.9106836025229592, 0.3333333333333335, 3], [0, 0, 0, 1]]
+```
+
+You can also set the orientation, translation, or matrix
+representation for an existing HomTran object using the setQuaternion,
+setTranslation, and setMatrix methods.
+
+Now for the useful part.  If you have a HomTran object that describes
+the location of a child frame relative to its parent frame, and you
+have a vector expressed in the child frame, you can get the
+corresponding vector in the parent frame using the transform method:
+
+```
+v = Vector[1, 2, 3]
+frame.transform(v)
+=> Vector[3.577350269189626, 2.8452994616207485, 5.577350269189626]
+```
+
+If you want the inverse of a HomTran, for example, to transform a
+vector from its representation in the parent frame to its
+representation in the child frame, you can use the inverse method:
+
+```
+frame.inverse()
+=> #<HomogeneousTransformation:0x000000025a7a10 @q=(0.7071067811865476, Vector[-0.408248290463863, -0.408248290463863, -0.408248290463863]), @t=Vector[-1.4226497308103743, -3.154700538379252, -1.4226497308103747]>
+```
+
+Finally, to compose two transformations described by HomTran objects,
+use the multiplication operator:
+
+```
+frame2 = HomTran.new(UnitQuaternion.new(1,2,3,4), Vector[1, 0, 0])
+=> #<HomogeneousTransformation:0x000000025bc820 @q=(0.18257418583505536, Vector[0.3651483716701107, 0.5477225575051661, 0.7302967433402214]), @t=Vector[1, 0, 0]>
+
+frame2 * frame
+#<HomogeneousTransformation:0x00000002334648 @q=(-0.5417209483763564, Vector[0.25819888974716115, 0.6109051323707207, 0.5163977794943223]), @t=Vector[2.7999999999999994, 2.0, 2.5999999999999996]>
+```
+
+Note that for any frame, frame * frame.inverse() (or frame.inverse() *
+frame) must yield the identity transformation:
+
+```
+result = frame * frame.inverse()
+=> #<HomogeneousTransformation:0x000000024c0a20 @q=(1.0, Vector[0.0, 0.0, 0.0]), @t=Vector[-4.440892098500626e-16, -4.440892098500626e-16, -8.881784197001252e-16]>
+
+result.getMatrix()
+=> Matrix[[1.0, 0.0, 0.0, -4.440892098500626e-16], [0.0, 1.0, 0.0, -4.440892098500626e-16], [0.0, 0.0, 1.0, -8.881784197001252e-16], [0, 0, 0, 1]]
+```
+
+## Advanced Features
+
+The initialize, setQuaternion, and setTranslation methods all accept
+two additional parameters: a HomTran object called rel_to, and a
+boolean value called local.  The value of local is only used if rel_to
+is not nil.
+
+By default, when initializing a HomTran object, or setting its
+orientation or position, this class assumes that the transformation is
+being specified relative to the "global" or "inertial" reference
+frame, or some other common parent frame.  However, if a value for
+rel_to is passed to any of the methods mentioned above, the
+orientation and rotation are specified relative to the rel_to frame.
+
+If local is true (its default value), then the location and
+orientation should be specified in the rel_to frame.  If local is
+false, they should be specified in the "global" frame (or rel_to's
+parent frame).
+
+Perhaps an example will clarify things.  Consider the following frame:
+
+```
+frame1 = HomTran.new(UnitQuaternion.fromAngleAxis(Math::PI/2, Vector[0, 0, 1]), Vector[1, 0, 0])
+=> #<HomogeneousTransformation:0x00000002451490 @q=(0.7071067811865476, Vector[0.0, 0.0, 0.7071067811865475]), @t=Vector[1, 0, 0]>
+```
+
+The frame is rotated by PI/2 radians about the global z-axis, and
+offset by 1 unit along the global x-axis.  Next, say we want to create
+a new frame with the same orientation, but offset by another 1 unit
+along the global x-axis.  We could create such a frame as follows:
+
+```
+frame2 = HomTran.new(UnitQuaternion.new(), Vector[1, 0, 0], frame1, false)
+=> #<HomogeneousTransformation:0x000000025d62c0 @q=(0.7071067811865476, Vector[0.0, 0.0, 0.7071067811865475]), @t=Vector[2, 0, 0]>
+
+frame2.getTranslation()
+=> Vector[2, 0, 0]
+```
+
+Notice that we specified [1, 0, 0] for frame2's translation, but,
+since we passed in frame1 for the rel_to argument, the resulting
+translation is [1, 0, 0] plus frame1's translation.  Also, notice that
+local was false, so we translated frame2 along the global x-axis.
+
+Since frame1 is rotated by PI/2 about the global z-axis, frame2's
+x-axis is aligned with the global y-axis.  So, if we create a new
+frame similar to frame2, but with local = true, we will translate it
+along frame1's x-axis, which aligns with the global y-axis.  So, the
+resulting translation should be [1, 1, 0].  Let's try it:
+
+```
+frame3 = HomTran.new(UnitQuaternion.new(), Vector[1, 0, 0], frame1, true)
+=> #<HomogeneousTransformation:0x000000025e7228 @q=(0.7071067811865476, Vector[0.0, 0.0, 0.7071067811865475]), @t=Vector[1.0000000000000002, 1.0, 0.0]>
+
+frame3.getTranslation()
+=> Vector[1.0000000000000002, 1.0, 0.0]
+```
+
+Exactly what we expected!
+
+A note of caution: when you create a HomTran using the rel_to
+parameter, this class calculates the resulting offset and orientation
+relative to the "global" frame and returns the corresponding HomTran.
+So, any subsequent changes to the rel_to frame will not affect the
+child frame, unless you update it manually using the setQuaternion or
+setTranslation methods with the rel_to parameter.
+
+## Contributing
+
+1. Fork it
+2. Create your feature branch (`git checkout -b my-new-feature`)
+3. Commit your changes (`git commit -am 'Add some feature'`)
+4. Push to the branch (`git push origin my-new-feature`)
+5. Create new Pull Request

data/Rakefile

@@ -0,0 +1,5 @@
+require "bundler/gem_tasks"
+
+task :test do
+     ruby 'tests/tests_HomogeneousTransformation.rb'
+end

data/homogeneous_transformation.gemspec

@@ -0,0 +1,28 @@
+# coding: utf-8
+lib = File.expand_path('../lib', __FILE__)
+$LOAD_PATH.unshift(lib) unless $LOAD_PATH.include?(lib)
+require 'homogeneous_transformation/version'
+
+Gem::Specification.new do |spec|
+  spec.name          = "homogeneous_transformation"
+  spec.version       = HomogeneousTransformation::VERSION
+  spec.authors       = ["Cory Crean"]
+  spec.email         = ["cory.crean@gmail.com"]
+  spec.description   = %q{Provides a HomogeneousTransformation class to convert back and forth between the representations of vectors in different reference frames}
+  spec.summary       = %q{Provides a HomogeneousTransformation class}
+  spec.homepage      = "https://github.com/ccrean/HomTran_ruby"
+  spec.license       = "BSD"
+
+  spec.files         = `git ls-files`.split($/)
+  spec.executables   = spec.files.grep(%r{^bin/}) { |f| File.basename(f) }
+  spec.test_files    = spec.files.grep(%r{^(test|spec|features)/})
+  spec.require_paths = ["lib"]
+
+  spec.required_ruby_version = ">=1.9.3"
+
+  spec.add_development_dependency "bundler", "~> 1.3"
+  spec.add_development_dependency "rake"
+  spec.add_development_dependency "simplecov", "~> 0.11.2"
+  spec.add_development_dependency "simplecov-html", "~> 0.10.0"
+  spec.add_runtime_dependency "unit_quaternion", ">= 0.0.5"
+end

data/lib/homogeneous_transformation.rb

@@ -0,0 +1,136 @@
+# Author:: Cory Crean (mailto:cory.crean@gmail.com)
+# Copyright:: Copyright (c) 2016 Cory Crean
+# License:: BSD
+#
+# A homogeneous transformation class, for converting between the
+# representations of vectors in different reference frames.
+
+require_relative 'homogeneous_transformation/version'
+require 'unit_quaternion'
+require 'matrix'
+
+class HomogeneousTransformation
+  # Creates a new HomogeneousTransformation from a quaternion and a vector.
+  #
+  # Params:
+  # +q+:: A UnitQuaternion object representing this frame's orientation relative to the rel_to frame.
+  # +translation+:: A 3-vector representing this frame's translation relative to the rel_to frame.
+  # +rel_to+:: A HomogeneousTransformation object relative to which the current frame's orientation and translation are specified.  If rel_to is nil, the orientation and translation will be specified relative to the global reference frame.
+  # +local+:: A boolean value.  If local is false, the relative position and orientation will be measured in the global reference frame.  If local is true, the relative position and orientation will be meausured in rel_to's reference frame.
+  def initialize(q = UnitQuaternion.new(1, 0, 0, 0),
+                 translation = Vector[0, 0, 0],
+                 rel_to = nil, local = true)
+    setQuaternion(q, rel_to, local)
+    setTranslation(translation, rel_to, local)
+  end
+
+  # Creates a homogeneous transformation given a suitable 4x4 matrix.
+  #
+  # Params:
+  # +m+:: A 4x4 matrix.  The upper left 3x3 submatrix must be orthonormal, and the final row must be [0, 0, 0, 1].
+  def self.fromMatrix(m)
+    h = HomogeneousTransformation.new()
+    h.setMatrix(m)
+    return h
+  end
+
+  # Returns the quaternion that represents the orientation of the homogeneous transformation.
+  def getQuaternion()
+    return @q
+  end
+
+  # Returns the 3-vector that specifies the translation of the homogeneous transformation.
+  def getTranslation()
+    return @t
+  end
+
+  # Sets the quaternion for the homogeneous transformation.
+  #
+  # Params:
+  # +q+:: A UnitQuaternion object representing the orientation of this frame.
+  # +rel_to+:: A HomogeneousTransformation object relative to which the orientation is specified.  If rel_to is nil, the orientation will be specified relative to the global reference frame.
+  # +local+:: A boolean value.  If local is false, the quaternion q specifies the orientation relative to rel_to as measured in the global coordinate frame.  If local is true, then q specifies the orientation relative to rel_to as measured in rel_to's coordinate frame.
+  def setQuaternion(q, rel_to = nil, local = true)
+    if rel_to
+      if local
+        @q = rel_to.getQuaternion() * q
+      else
+        @q = q * rel_to.getQuaternion()
+      end
+    else
+      @q = q
+    end
+  end
+
+  # Sets the translation for the homogeneous transformation.
+  #
+  # Params:
+  # +t+:: A vector describing the frame's translation.
+  # +rel_to+:: A HomogeneousTransformation object relative to which the translation is specified.  If rel_to is nil, the translation will be specified relative to the global reference frame.
+  # +local+:: A boolean value.  If local is false, then t specifies the translation relative to rel_to as measured in the global coordinate frame.  If local is true, then t specifies the translation relative to rel_to as measured in rel_to's coordinate frame.
+  def setTranslation(t, rel_to = nil, local = true)
+    if rel_to
+      if local
+        @t = rel_to.getTranslation() +
+          rel_to.getQuaternion().transform(t)
+      else
+        @t = rel_to.getTranslation() + t
+      end
+    else
+      @t = t
+    end
+  end
+
+  # Sets the value of the homogeneous transformation given a suitable 4x4 matrix.
+  #
+  # Params:
+  # +m+:: A 4x4 matrix.  The upper left 3x3 submatrix must be orthonormal, and the final row must be [0, 0, 0, 1].
+  def setMatrix(m)
+    if m.row_size() != 4 or m.column_size() != 4
+      raise(ArgumentError, "Matrix must be 4x4")
+    end
+    if (m.row(3) - Vector[0,0,0,1]).norm() > 1e-15
+      raise(ArgumentError, "Final row must be [0, 0, 0, 1]")
+    end
+    @q.setRotationMatrix(m.minor(0..2, 0..2))
+    @t = m.column(3)[0..2]
+  end
+
+  # Takes a vector v representing a point in the local frame and returns the
+  # vector to that same point relative to the global frame.
+  #
+  # Params:
+  # +v+:: A vector in the local reference frame.
+  #
+  # Returns:
+  # The representation of v in the global reference frame.
+  def transform(v)
+    return @t + @q.transform(v)
+  end
+
+  # Returns the inverse of the homogeneous transformation.
+  def inverse
+    q_inv = @q.inverse()
+    t_inv = q_inv.transform(-1 * @t)
+    return HomogeneousTransformation.new(q_inv, t_inv)
+  end
+
+  # Returns the 4x4 matrix representation of the homogeneous transformation.
+  def getMatrix
+    rot_mat = @q.getRotationMatrix()
+    m = Matrix.columns([*rot_mat.transpose(), @t])
+    m = Matrix.rows([*m, [0, 0, 0, 1]])
+    return m
+  end
+
+  # Compose two homogeneous transformations.
+  def *(other)
+    q_result = self.getQuaternion() * other.getQuaternion()
+    t_result = self.getQuaternion().transform(other.getTranslation()) +
+      self.getTranslation()
+    result = HomogeneousTransformation.new(q_result, t_result)
+    return result
+  end
+end
+
+HomTran = HomogeneousTransformation

data/lib/homogeneous_transformation/version.rb

@@ -0,0 +1,3 @@
+class HomogeneousTransformation
+  VERSION = "0.0.1"
+end

data/tests/tests_HomogeneousTransformation.rb

@@ -0,0 +1,234 @@
+# Name: tests_HomogeneousTransformation.rb
+# Description: Test cases for the HomogeneousTransformation class
+# Author: Cory Crean
+# E-mail: cory.crean@gmail.com
+# Copyright (c) 2016, Cory Crean
+
+require 'simplecov'
+SimpleCov.start
+require 'test/unit'
+require 'matrix'
+require 'unit_quaternion'
+require_relative '../lib/homogeneous_transformation'
+
+def isIdentityMatrix(m, tol)
+  for i, j in [0,1,2].product([0,1,2])
+    if i == j
+      if (m[i,j] - 1).abs > tol
+        return false
+      end
+    else
+      if m[i,j].abs > tol
+        return false
+      end
+    end
+  end
+  return true
+end
+
+def areEqualMatrices(m1, m2, tol)
+  m1.zip(m2).each() do |v1, v2|
+    if (v1 - v2).abs() > tol
+      puts("error = ", (v1 - v2).abs())
+      return false
+    end
+  end
+  return true
+end
+
+def buildMatrix(q, t)
+  m = q.getRotationMatrix()
+  m = Matrix[*m.transpose(), t].transpose()
+  m = Matrix[*m, [0, 0, 0, 1]]
+end
+
+class TestHomogeneousTransformation < Test::Unit::TestCase
+
+  def setup
+    @quats = [ ::UnitQuaternion.new(1,2,3,4),
+               ::UnitQuaternion.new(0.1, 0.01, 2.3, 4),
+               ::UnitQuaternion.new(1234.4134, 689.6124, 134.124, 0.5),
+               ::UnitQuaternion.new(1,1,1,1),
+               UnitQuaternion.new(1, 0, 0, 0),
+             ]
+    @angles = [ 2*Math::PI, Math::PI, Math::PI/2, Math::PI/4,
+                0.5,  0.25, 0.1234, ]
+    @axes = [ Vector[ 1, 1, 1 ], Vector[ 1, 0, 0 ], Vector[ 0, 1, 0 ],
+              Vector[ 0, 0, 1 ], Vector[ 1, 2, 3 ], ]
+    for angle, axis in @angles.product(@axes)
+      @quats << UnitQuaternion.fromAngleAxis(angle, axis)
+    end
+
+    positions = (0..1).step(0.5).to_a
+    @translations = []
+    positions.product(positions, positions).each() do |a, b, c|
+      @translations << Vector[a, b, c]
+    end
+  end
+
+  def test_initialize
+    ht = HomTran.new()
+    assert_equal(ht.getQuaternion(), UnitQuaternion.new(1, 0, 0, 0))
+    assert_equal(ht.getTranslation(), Vector[0, 0, 0])
+
+    q = UnitQuaternion.new(1,2,3,4)
+    t = Vector[5,6,7]
+    ht = HomTran.new(q, t)
+    assert_equal(ht.getQuaternion(), q)
+    assert_equal(ht.getTranslation(), t)
+    
+    @quats.product(@translations).each() do |q2, t2|
+      fr = HomTran.new(q2, t2, ht)
+      assert_in_delta( (fr.getQuaternion() - q * q2).norm(), 0, 1e-15)
+      assert_in_delta( (fr.getTranslation() - (t + q.transform(t2))).norm(),
+                       0, 1e-15)
+
+      fr = HomTran.new(q2, t2, ht, false)
+      assert_in_delta( (fr.getQuaternion() - q2 * q).norm(), 0, 1e-15)
+      assert_in_delta( (fr.getTranslation() - (t + t2)).norm(), 0, 1e-15)
+    end
+  end
+
+  def test_setQuaternion
+    for q in @quats
+      fr1 = HomTran.new()
+      fr1.setQuaternion(q)
+      assert_equal(fr1.getQuaternion(), q)
+    end
+    for q1, q2 in @quats.product(@quats)
+      fr1 = HomTran.new(q1)
+      fr2 = HomTran.new()
+      fr2.setQuaternion(q2, fr1)
+      assert_in_delta( (fr2.getQuaternion() - q1 * q2).norm(), 0, 1e-15 )
+      
+      fr2.setQuaternion(q2, fr1, false)
+      assert_in_delta( (fr2.getQuaternion() - q2 * q1).norm(), 0, 1e-15 )
+    end
+  end
+
+  def test_setTranslation
+    @quats.product(@translations).each() do |q, t|
+      fr1 = HomTran.new(q)
+      fr1.setTranslation(t)
+      assert_in_delta( (fr1.getTranslation() - t).norm(), 0, 1e-15 )
+
+      fr2 = HomTran.new()
+      fr2.setTranslation(t, fr1)
+      assert_in_delta( (fr1.getTranslation() +
+                        fr1.getQuaternion().transform(t) - 
+                        fr2.getTranslation()).norm(), 0, 1e-15 )
+
+      fr2.setTranslation(t, fr1, false)
+      assert_in_delta( (fr2.getTranslation - 2 * t).norm(), 0, 1e-15)
+    end
+  end
+
+  def test_transform
+    t = Vector[1,2,3]
+    fr = HomTran.new(UnitQuaternion.fromAngleAxis(Math::PI/2,
+                                                  Vector[0, 0, 1]), t)
+    assert_in_delta( (fr.transform(Vector[2, 0, 0]) -
+                      (t + Vector[0, 2, 0])).norm(), 0, 1e-15)
+    assert_in_delta( (fr.transform(Vector[0, 2, 0]) -
+                      (t + Vector[-2, 0, 0])).norm(), 0, 1e-15)
+
+    fr = HomTran.new(UnitQuaternion.fromAngleAxis(Math::PI/2,
+                                                  Vector[0, 1, 0]), t)
+    assert_in_delta( (fr.transform(Vector[2, 0, 0]) -
+                      (t + Vector[0, 0, -2])).norm(), 0, 1e-15)
+    assert_in_delta( (fr.transform(Vector[0, 0, 2]) -
+                      (t + Vector[2, 0, 0])).norm(), 0, 1e-15)
+
+    fr = HomTran.new(UnitQuaternion.fromAngleAxis(Math::PI/2,
+                                                  Vector[1, 0, 0]),
+                     t)
+    assert_in_delta( (fr.transform(Vector[2, 0, 0]) -
+                      (t + Vector[2, 0, 0])).norm(), 0, 1e-15)
+    assert_in_delta( (fr.transform(Vector[0, 2, 0]) -
+                      (t + Vector[0, 0, 2])).norm(), 0, 1e-15)
+    assert_in_delta( (fr.transform(Vector[0, 0, 2]) -
+                      (t + Vector[0, -2, 0])).norm(), 0, 1e-15)
+  end
+
+  def test_inverse
+    vectors = [ Vector[1,2,3], Vector[0,0,1], Vector[0.1,0.1,0.1] ]
+    @quats.product(@translations).each() do |q, t|
+      fr = HomTran.new(q, t)
+      vectors.each() do |v|
+        transformed = fr.inverse().transform(fr.transform(v))
+        assert_in_delta( (v - transformed).norm(), 0, 1e-14)
+      end
+    end
+  end
+
+  def test_matrix
+    @quats.product(@translations).each() do |q, t|
+      fr = HomTran.new(q, t)
+      fr_inv = fr.inverse()
+      assert(isIdentityMatrix( fr.getMatrix() * fr_inv.getMatrix(), 1e-15))
+    end
+
+    @quats.product(@translations).each() do |q, t|
+      fr = HomTran.new()
+      fr.setMatrix(buildMatrix(q, t))
+      assert(areEqualMatrices(fr.getQuaternion().getRotationMatrix(),
+                              q.getRotationMatrix(), 1e-15))
+      assert(areEqualMatrices(fr.getTranslation(), t, 1e-15))
+    end
+
+    @quats.product(@translations).each() do |q, t|
+      fr = HomTran.fromMatrix(buildMatrix(q, t))
+      assert(areEqualMatrices(fr.getQuaternion().getRotationMatrix(),
+                              q.getRotationMatrix(), 1e-15))
+      assert(areEqualMatrices(fr.getTranslation(), t, 1e-15))
+    end
+
+    q = UnitQuaternion.new(1,2,3,4)
+    t = Vector[1,2,3]
+    m = buildMatrix(q, t)
+    m2 = Matrix[m.row(0), m.row(1), m.row(2), [0.01, 0, 0, 1]]
+    ht = HomTran.new()
+    assert_raise(ArgumentError) do
+      ht.setMatrix(m2)
+    end
+    assert_raise(ArgumentError) do
+      HomTran.fromMatrix(m2)
+    end
+
+    m2 = Matrix[m.row(0), m.row(1), m.row(2), [0, 0, 0, 1.0001]]
+    assert_raise(ArgumentError) do
+      ht.setMatrix(m2)
+    end
+    assert_raise(ArgumentError) do
+      HomTran.fromMatrix(m2)
+    end
+
+    m2 = Matrix[ [1, 0.001, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3],
+                 [0, 0, 0, 1] ]
+    assert_raise(ArgumentError) do
+      ht.setMatrix(m2)
+    end
+    assert_raise(ArgumentError) do
+      HomTran.fromMatrix(m2)
+    end
+
+    m2 = Matrix[ [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0] ]
+    assert_raise(ArgumentError) do
+      ht.setMatrix(m2)
+    end
+    assert_raise(ArgumentError) do
+      HomTran.fromMatrix(m2)
+    end
+  end
+
+  def test_multiply
+    parts = @quats.product(@translations)
+    for i in 0...(parts.length() - 1)
+      fr1 = HomTran.new(*parts[i])
+      fr2 = HomTran.new(*parts[i+1])
+      result = fr1 * fr2
+      assert(areEqualMatrices(result.getMatrix(),
+                              fr1.getMatrix() * fr2.getMatrix(), 1e-15))
+    end
+  end
+end

metadata

@@ -0,0 +1,136 @@
+--- !ruby/object:Gem::Specification
+name: homogeneous_transformation
+version: !ruby/object:Gem::Version
+  version: 0.0.1
+  prerelease: 
+platform: ruby
+authors:
+- Cory Crean
+autorequire: 
+bindir: bin
+cert_chain: []
+date: 2016-08-04 00:00:00.000000000 Z
+dependencies:
+- !ruby/object:Gem::Dependency
+  name: bundler
+  requirement: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: '1.3'
+  type: :development
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: '1.3'
+- !ruby/object:Gem::Dependency
+  name: rake
+  requirement: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ! '>='
+      - !ruby/object:Gem::Version
+        version: '0'
+  type: :development
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ! '>='
+      - !ruby/object:Gem::Version
+        version: '0'
+- !ruby/object:Gem::Dependency
+  name: simplecov
+  requirement: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: 0.11.2
+  type: :development
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: 0.11.2
+- !ruby/object:Gem::Dependency
+  name: simplecov-html
+  requirement: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: 0.10.0
+  type: :development
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ~>
+      - !ruby/object:Gem::Version
+        version: 0.10.0
+- !ruby/object:Gem::Dependency
+  name: unit_quaternion
+  requirement: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ! '>='
+      - !ruby/object:Gem::Version
+        version: 0.0.5
+  type: :runtime
+  prerelease: false
+  version_requirements: !ruby/object:Gem::Requirement
+    none: false
+    requirements:
+    - - ! '>='
+      - !ruby/object:Gem::Version
+        version: 0.0.5
+description: Provides a HomogeneousTransformation class to convert back and forth
+  between the representations of vectors in different reference frames
+email:
+- cory.crean@gmail.com
+executables: []
+extensions: []
+extra_rdoc_files: []
+files:
+- .gitignore
+- Gemfile
+- LICENSE.txt
+- README.md
+- Rakefile
+- homogeneous_transformation.gemspec
+- lib/homogeneous_transformation.rb
+- lib/homogeneous_transformation/version.rb
+- tests/tests_HomogeneousTransformation.rb
+homepage: https://github.com/ccrean/HomTran_ruby
+licenses:
+- BSD
+post_install_message: 
+rdoc_options: []
+require_paths:
+- lib
+required_ruby_version: !ruby/object:Gem::Requirement
+  none: false
+  requirements:
+  - - ! '>='
+    - !ruby/object:Gem::Version
+      version: 1.9.3
+required_rubygems_version: !ruby/object:Gem::Requirement
+  none: false
+  requirements:
+  - - ! '>='
+    - !ruby/object:Gem::Version
+      version: '0'
+requirements: []
+rubyforge_project: 
+rubygems_version: 1.8.23
+signing_key: 
+specification_version: 3
+summary: Provides a HomogeneousTransformation class
+test_files: []