eigen 0.1.0

data/ext/eigen/extconf.rb

@@ -0,0 +1,10 @@
+require "mkmf-rice"
+
+RbConfig::CONFIG['CXXFLAGS'].gsub! "-Wdeclaration-after-statement", ''
+RbConfig::CONFIG['CXXFLAGS'].gsub! "-Wimplicit-function-declaration", ''
+
+if !pkg_config('eigen3')
+    raise "cannot find the eigen3 pkg-config package in #{ENV['PKG_CONFIG_PATH']}"
+end
+
+create_makefile("eigen/eigen")

data/lib/eigen.rb

@@ -0,0 +1,15 @@
+# Ruby bindings for the Eigen C++ linear algebra library
+module Eigen
+end
+
+require 'eigen/eigen'
+
+require 'eigen/affine3'
+require 'eigen/angle_axis'
+require 'eigen/isometry3'
+require 'eigen/matrix4'
+require 'eigen/matrixx'
+require 'eigen/quaternion'
+require 'eigen/vector3'
+require 'eigen/vectorx'
+require 'eigen/version'

data/lib/eigen/affine3.rb

@@ -0,0 +1,35 @@
+module Eigen
+    class Affine3
+        def self.Identity
+            Affine3.new
+        end
+
+        def self.from_position_orientation( v, q )
+            i = Affine3.Identity
+            i.prerotate( q )
+            i.pretranslate( v )
+            i
+        end
+
+        def dup
+            raise NotImplementedError
+        end
+
+        def ==(q)
+            q.kind_of?(self.class) &&
+                __equal__(q)
+        end
+
+        def *(obj)
+            if obj.kind_of?(Affine3)
+                concatenate(obj)
+            else
+                transform(obj)
+            end
+        end
+
+        def to_s
+            matrix.to_s
+        end
+    end
+end

data/lib/eigen/angle_axis.rb

@@ -0,0 +1,90 @@
+module Eigen
+    # Representation and manipulation of an angle axis
+    class AngleAxis
+        def dup
+            AngleAxis.new(angle, axis)
+        end
+
+        # Returns the angle axis as [angle, x, y, z]
+        def to_a; [angle, axis.to_a] end
+
+        def from_a (array)
+            aa = AngleAxis.new(array[0], Eigen::Vector3.new(array[1][0], array[1][1], array[1][2]))
+            aa
+        end
+
+        # Returns the identity unit quaternion (identity rotation)
+        def self.Identity
+            AngleAxis.new(0, Eigen::Vector3.new(1, 0, 0))
+        end
+
+        # Creates a angle axis from a quaternion
+        def self.from_quaternion(*args)
+            aa = new(0, Eigen::Vector3.new(1, 0, 0))
+            aa.from_quaternion(*args)
+            aa
+        end
+
+        # Creates a quaternion from a set of euler angles.
+        #
+        # See Quaternion#from_euler for details
+        def self.from_euler(*args)
+            aa = new(0, Eigen::Vector3.new(1, 0, 0))
+            aa.from_euler(*args)
+            aa
+        end
+
+        # Creates a quaternion from a rotation matrix
+        def self.from_matrix(m)
+            aa = new(0, Eigen::Vector3.new(1, 0, 0))
+            aa.from_matrix(m)
+            aa
+        end
+
+        # Returns a scaled axis representation that is equivalent to this
+        # quaternion
+        #
+        # @param [Float] eps see {#to_angle_axis}
+        # @return [Vector3]
+        def to_scaled_axis(eps = 1e-12)
+            return axis * angle
+        end
+
+        # Concatenates with another angle axis or transforms a vector
+        def *(obj)
+            if obj.kind_of?(AngleAxis)
+                concatenate(obj)
+            else
+                transform(obj)
+            end
+        end
+
+        def _dump(level) # :nodoc:
+            Marshal.dump(to_a)
+        end
+
+        def self._load(coordinates) # :nodoc:
+            aa = new(0, Eigen::Vector3.new(1, 0, 0))
+            aa.from_a(Marshal.load(coordinates))
+            aa
+        end
+
+        def to_s # :nodoc:
+            "AngleAxis( angle #{angle}, axis(#{axis.x}, #{axis.y}, #{axis.z}))"
+        end
+
+        # Tests for equality
+        #
+        # Since Quaternion stores the coordinates as floating-point values, this is
+        # a bad test. Use
+        #
+        #   (v - other_v).norm < threshold
+        #
+        # instead
+        def ==(q)
+            q.kind_of?(self.class) &&
+                __equal__(q)
+        end
+    end
+end
+

data/lib/eigen/isometry3.rb

@@ -0,0 +1,36 @@
+module Eigen
+    class Isometry3
+        def self.Identity
+            Isometry3.new
+        end
+
+        def self.from_position_orientation( v, q )
+            i = Isometry3.Identity
+            i.prerotate( q )
+            i.pretranslate( v )
+            i
+        end
+
+        def dup
+            raise NotImplementedError
+        end
+
+        def ==(q)
+            q.kind_of?(self.class) &&
+                __equal__(q)
+        end
+
+        def *(obj)
+            if obj.kind_of?(Isometry3)
+                concatenate(obj)
+            else
+                transform(obj)
+            end
+        end
+
+        def to_s
+            matrix.to_s
+        end
+    end
+end
+

data/lib/eigen/matrix4.rb

@@ -0,0 +1,96 @@
+module Eigen
+    # Matrix 4x4
+    class Matrix4
+        # @!method [](row, col)
+        #   @param [Integer] row the row index
+        #   @param [Integer] col the column index
+        
+        # Create a new matrix from the content of an array
+        #
+        # @param [Array<Numeric>] the values. It must be of size at most 16.
+        #    If smaller than 16, the rest is filled with zeroes
+        #
+        # @return [Matrix4]
+        def self.from_a(*args)
+            m = new
+            m.from_a(*args)
+            m
+        end
+
+        # Returns the values flattened in a ruby array
+        #
+        # @param [Boolean] column_major if true, the values of a column will be
+        #   adjacent in the resulting array, if not the values of a row will
+        # @return [Array<Numeric>]
+        #
+        # @example column-major ordering
+        #   matrix.to_s => Matrix4(1 2 3 4
+        #                          5 6 7 8
+        #                          9 10 11 12
+        #                          13 14 15 16)
+        #   matrix.to_a(true) => [1 5 9 13 2 6 10 14 ...]
+        def to_a(column_major=true)
+            a = []
+            if column_major
+                for j in 0..3
+                    for i in 0..3
+                        a << self[i,j]
+                    end
+                end
+            else
+                for i in 0..3
+                    for j in 0..3
+                        a << self[i,j]
+                    end
+                end
+            end
+            a
+        end
+
+        # sets matrix from a 1d array
+        def from_a(array, column_major=true)
+            if array.size > 16
+                raise ArgumentError, "array should be of size maximum 16"
+            end
+
+            16.times do |i|
+                v = array[i] || 0
+                if column_major
+                    self[i%4,i/4] = v
+                else
+                    self[i/4,i%4] = v
+                end
+            end
+        end
+
+        def ==(m)
+            m.kind_of?(self.class) &&
+                __equal__(m)
+        end
+
+        def to_s(line_format = "%g %g %g %g") # :nodoc:
+            lines = to_a.each_slice(3).to_s
+            <<-EOSTRING
+Matrix4(#{line_format % lines[0]}"
+        #{line_format % lines[1]}"
+        #{line_format % lines[2]}"
+        #{line_format % lines[3]})"
+            EOSTRING
+        end
+
+        def _dump(level)
+            to_a.pack("E*")
+        end
+
+        def self._load(elements) # :nodoc:
+            m = new
+            if elements.size == 8 * 16
+                m.from_a(elements.unpack("E*"))
+            else
+                m.from_a(Marshal.load(elements)['data'])
+            end
+            m
+        end
+    end
+end
+

data/lib/eigen/matrixx.rb

@@ -0,0 +1,105 @@
+module Eigen
+    # Abritary size vector
+    class MatrixX
+        def dup
+            MatrixX.from_a(to_a, rows, cols)
+        end
+
+        def self.Zero(rows, cols)
+            m = new(rows, cols)
+            rows.times do |r|
+                cols.times do |c|
+                    m[r, c] = 0
+                end
+            end
+            m
+        end
+
+        def self.from_a(*args)
+            m = new
+            m.from_a(*args)
+            m
+        end
+
+        # Returns the array value in a vector 
+        def to_a(column_major=true)
+            a = []
+            if column_major
+                for j in 0..cols()-1
+                    for i in 0..rows()-1
+                        a << self[i,j]
+                    end
+                end
+            else
+                for i in 0..rows()-1
+                    for j in 0..cols()-1
+                        a << self[i,j]
+                    end
+                end
+            end
+            a
+        end
+
+        # sets matrix from a 1d array
+        def from_a(array,nrows=-1,ncols=-1,column_major=true)
+            if nrows == -1 && ncols == -1
+                nrows = rows
+                ncols = cols
+            elsif nrows == -1
+                nrows = array.size / ncols
+            elsif ncols == -1
+                ncols = array.size / nrows
+            end
+            resize(nrows,ncols)
+            array.each_index do |i|
+                v = array[i]
+                if !v
+                    v = 0.0
+                end
+                if column_major
+                    self[i%nrows,i/nrows] = v
+                else
+                    self[i/ncols,i%ncols] = v
+                end
+            end
+        end
+
+        def pretty_print(pp)
+            for i in 0..rows()-1
+                for j in 0..cols()-1
+                    pp.text " #{self[i,j]}"
+                end
+                pp.text "\n"
+            end
+        end
+
+        def ==(m)
+            m.kind_of?(self.class) &&
+                __equal__(m)
+        end
+
+        def to_s # :nodoc:
+            str = "MatrixX(\n"
+            for i in 0..rows()-1
+                for j in 0..cols()-1
+                    str += "#{self[i,j]} "
+                end
+                str[-1] = "\n"
+            end
+            str += ")"
+            str
+        end
+
+        def _dump(level) # :nodoc:
+            Marshal.dump({'rows' => rows, 'cols' => cols, 'data' => to_a})
+        end
+
+        def self._load(coordinates) # :nodoc:
+            o = Marshal.load(coordinates)
+            m = new(o['rows'],o['cols'])
+            m.from_a(o['data'],o['rows'],o['cols'])
+            m
+        end
+    end
+end
+

data/lib/eigen/quaternion.rb

@@ -0,0 +1,253 @@
+module Eigen
+    # Representation and manipulation of a quaternion
+    class Quaternion
+        def dup
+            Quaternion.new(w, x, y, z)
+        end
+
+        # Returns the quaternion as [w, x, y, z]
+        def to_a; [w, x, y, z] end
+
+        # Returns the identity unit quaternion (identity rotation)
+        def self.Identity
+                Quaternion.new(1, 0, 0, 0)
+        end
+
+        # DEPRECATED: please use identity instead. Returns the unit quaternion (identity rotation)
+        def self.Unit
+	    warn "[DEPRECATED] Quaternion.unit, please use Quaternion.identity."
+	    self.Identity
+        end
+
+        # Creates a quaternion from an angle and axis description 
+        def self.from_angle_axis(*args)
+                q = new(1, 0, 0, 0)
+            q.from_angle_axis(*args)
+            q
+        end
+
+        # Returns an angle,axis representation equivalent to this quaternion
+        #
+        # If the angle turns out to be PI, there are two solutions and the one
+        # with positive Z component is chosen.
+        #
+        # @param [Float] eps if the angle turns out to be closer to zero than eps, the
+        #   rotation axis is undefined and chosen to be the Z axis.
+        #
+        # @return [(Float,Vector3)] the angle and axis. The angle is in [0, PI]
+        def to_angle_axis(eps = 1e-12)
+            w, x, y, z = to_a
+            norm  = Math.sqrt(x*x+y*y+z*z);
+            if norm < eps
+                return 0, Eigen::Vector3.new(0,0,1);
+            end
+
+            angle = 2.0 * Math.atan2(norm, w);
+            axis  = Eigen::Vector3.new(x, y, z) / norm
+            return angle, axis
+        end
+
+        # Returns a scaled axis representation that is equivalent to this
+        # quaternion
+        #
+        # @param [Float] eps see {#to_angle_axis}
+        # @return [Vector3]
+        def to_scaled_axis(eps = 1e-12)
+            angle, axis = to_angle_axis(eps)
+            return axis * angle
+        end
+
+        # Creates a quaternion from a set of euler angles.
+        #
+        # See Quaternion#from_euler for details
+        def self.from_euler(*args)
+            q = new(1, 0, 0, 0)
+            q.from_euler(*args)
+            q
+        end
+
+        # Creates a quaternion from a rotation matrix
+        def self.from_matrix(m)
+            q = new(1, 0, 0, 0)
+            q.from_matrix(m)
+            q
+        end
+
+        # Extracts the yaw angle from this quaternion
+        #
+        # It decomposes the quaternion in euler angles using to_euler
+        # and returns the first element. See #to_euler for details.
+        def yaw
+            to_euler[0]
+        end
+
+        def pitch
+            to_euler[1]
+        end
+
+        def roll
+            to_euler[2]
+        end
+
+        # The inverse of #yaw
+        def self.from_yaw(yaw)
+            from_euler(Eigen::Vector3.new(yaw, 0, 0), 2, 1, 0)
+        end
+
+        # Concatenates with another quaternion or transforms a vector
+        def *(obj)
+            if obj.kind_of?(Quaternion)
+                concatenate(obj)
+            else
+                transform(obj)
+            end
+        end
+
+        def _dump(level) # :nodoc:
+            Marshal.dump(to_a)
+        end
+
+        def self._load(coordinates) # :nodoc:
+            new(*Marshal.load(coordinates))
+        end
+
+        def to_s # :nodoc:
+            "Quaternion(#{w}, (#{x}, #{y}, #{z}))"
+        end
+
+        # Tests for equality
+        #
+        # Since Quaternion stores the coordinates as floating-point values, this is
+        # a bad test. Use
+        #
+        #   (v - other_v).norm < threshold
+        #
+        # instead
+        def ==(q)
+            q.kind_of?(self.class) &&
+                __equal__(q)
+        end
+
+        def re
+            w
+        end
+
+        def re=(value)
+            self.w = value
+        end
+
+        def im
+            [x,y,z]
+        end
+
+        def im=(value)
+            self.x, self.y, self.z = *value
+        end
+
+        ##
+        # :method: w
+
+        ##
+        # :method: x
+
+        ##
+        # :method: y
+
+        ##
+        # :method: z
+
+        ##
+        # :method: w=
+
+        ##
+        # :method: x=
+
+        ##
+        # :method: y=
+
+        ##
+        # :method: z=
+
+        ##
+        # :method: concatenate
+        # :call-seq:
+        #   concatenate(q)
+        #
+        # Returns the rotation in which +q+ is applied first and +self+ second
+
+        ##
+        # :method: transform
+        # :call-seq:
+        #   transform(v)
+        #
+        # Transforms the given Eigen::Vector3 by the rotation represented with
+        # this quaternion
+
+        ##
+        # :method: normalize!
+        # 
+        # Normalizes this quaternion
+
+        ##
+        # :method: normalize
+        # 
+        # Returns a quaternion that is a normalized version of +self+
+
+        ##
+        # :method: approx?
+        # :call-seq:
+        #   approx?(q, tolerance)
+        # 
+        # Returns true if +self+ and +q+ do not differ from more than
+        # +tolerance+. The comparison is done on a coordinate basis.
+
+        ##
+        # :method: to_euler
+        # :call-seq:
+        #    to_euler => Eigen::Vector3(a0, a1, a2)
+        # 
+        # Decomposes this quaternion in euler angles so that +self+ can be
+        # obtained by applying the following rotations in order:
+        #
+        #   rotation of a2 around x-axis
+        #   rotation of a1 around y-axis
+        #   rotation of a0 around z-axis
+        #
+        #   assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2)
+        #
+        # note that 
+        #
+        #   self == Quaternion.from_euler(to_euler, axis0, axis1, axis2)
+
+        ##
+        # :method: from_euler
+        # :call-seq:
+        #    from_euler(Eigen::Vector3(a0, a1, a2), axis0, axis1, axis2)
+        # 
+        # Resets this quaternion so that it represents the rotation obtained by
+        # applying the following rotations in order:
+        #
+        #   rotation of a2 around axis2
+        #   rotation of a1 around axis1
+        #   rotation of a0 around axis0
+        #
+        # note that 
+        #
+        #   self == Quaternion.from_euler(to_euler, axis0, axis1, axis2)
+
+        ## 
+        # :method: inverse
+        # :call-seq:
+        #   inverse => quaternion
+        #
+        # Computes the quaternion that is inverse of this one
+
+
+        # @return [Qt::Quaternion] the Qt quaternion that is identical to this
+        # one
+        def to_qt
+            Qt::Quaternion.new(w, x, y, z)
+        end
+    end
+end
+