gmath3D 0.2.0 → 0.2.1

data/VERSION

@@ -1 +1 @@
-0.2.0
+0.2.1

data/gmath3D.gemspec

@@ -5,11 +5,11 @@
 
 Gem::Specification.new do |s|
   s.name = %q{gmath3D}
-  s.version = "0.2.0"
+  s.version = "0.2.1"
 
   s.required_rubygems_version = Gem::Requirement.new(">= 0") if s.respond_to? :required_rubygems_version=
   s.authors = ["Toshiyasu Shimizu"]
-  s.date = %q{2011-10-09}
+  s.date = %q{2011-10-19}
   s.description = %q{This library defines 3D geometric elements(point, line, plane etc..). It can get two(or more) elements relation, like distance between two elements.}
   s.email = %q{toshi0328@gmail.com}
   s.extra_rdoc_files = [
@@ -30,7 +30,9 @@ Gem::Specification.new do |s|
     "lib/geom.rb",
     "lib/gmath3D.rb",
     "lib/line.rb",
+    "lib/matrix_util.rb",
     "lib/plane.rb",
+    "lib/quat.rb",
     "lib/rectangle.rb",
     "lib/triangle.rb",
     "lib/util.rb",
@@ -39,9 +41,10 @@ Gem::Specification.new do |s|
     "test/test_box.rb",
     "test/test_finite_line.rb",
     "test/test_geom.rb",
-    "test/test_gmath3D.rb",
     "test/test_line.rb",
+    "test/test_matrix_util.rb",
     "test/test_plane.rb",
+    "test/test_quat.rb",
     "test/test_rectangle.rb",
     "test/test_triangle.rb",
     "test/test_util.rb",

data/lib/finite_line.rb

@@ -10,21 +10,33 @@ public
     attr_accessor :end_point
 
     # [Input]
-    #  _start_point_arg_ and _end_point_arg_ should be Vector3.
+    #  _start_point_ and _end_point_ should be Vector3.
     # [Output]
     #  return new instance as FiniteLine
-    def initialize(start_point_arg = Vector3.new(0.0,0.0,0.0), end_point_arg = Vector3.new(1.0,0.0,0.0))
-      Util.check_arg_type(Vector3, start_point_arg)
-      Util.check_arg_type(Vector3, end_point_arg)
+    def initialize(start_point = Vector3.new(0.0,0.0,0.0), end_point = Vector3.new(1.0,0.0,0.0))
+      Util.check_arg_type(Vector3, start_point)
+      Util.check_arg_type(Vector3, end_point)
       super()
-      @start_point = start_point_arg
-      @end_point  = end_point_arg
+      @start_point = start_point
+      @end_point  = end_point
     end
 
     def to_s
       "FiniteLine[from#{start_point.to_element_s}, to#{end_point.to_element_s}]"
     end
 
+    # [Input]
+    #  _rhs_ should be FiniteLine.
+    # [Output]
+    #  return true if rhs equals myself.
+    def ==(rhs)
+      return false if rhs == nil
+      Util.check_arg_type(FiniteLine, rhs)
+      return false if( self.start_point != rhs.start_point)
+      return false if( self.end_point != rhs.end_point)
+      return true
+    end
+
     # [Output]
     #  return direction as vector from start_point to end_point as Vector3
     def direction
@@ -87,6 +99,30 @@ public
       closest_point = points_ary[distance_ary.index(distance)]
       return distance, closest_point
     end
+
+    def self.ary_distance_to_line(finite_lines, target_line)
+      Util.check_arg_type(::Array, finite_lines)
+      Util.check_arg_type(Line, target_line)
+      distance_ary = Array.new(0)
+      point_on_target_ary = Array.new(0)
+      point_on_finite_line_ary = Array.new(0)
+      param_on_target_ary = Array.new(0)
+      param_on_finite_line_ary = Array.new(0)
+      finite_lines.each do | item |
+        distance, point_on_myself, point_on_target, parameter_on_myself, parameter_on_tatget = item.distance(target_line)
+        distance_ary.push(distance)
+        point_on_target_ary.push(point_on_target)
+        point_on_finite_line_ary.push(point_on_myself)
+        param_on_target_ary.push(parameter_on_tatget)
+        param_on_finite_line_ary.push(parameter_on_myself)
+      end
+      distance = distance_ary.min
+      point_on_finiteline = point_on_finite_line_ary[distance_ary.index(distance)]
+      point_on_target = point_on_target_ary[distance_ary.index(distance)]
+      param_on_finiteline = param_on_finite_line_ary[distance_ary.index(distance)]
+      param_on_target = param_on_target_ary[distance_ary.index(distance)]
+      return distance, point_on_finiteline, point_on_target, param_on_finiteline, param_on_target
+    end
 private
     def distance_to_point(target)
       # get distance using infinite line

data/lib/gmath3D.rb

@@ -3,6 +3,9 @@ $LOAD_PATH.unshift(File.dirname(__FILE__))
 require 'util'
 require 'geom'
 require 'vector3'
+require 'quat'
+require 'matrix_util'
+
 require 'line'
 require 'finite_line'
 require 'plane'

data/lib/line.rb

@@ -9,6 +9,10 @@ module GMath3D
     attr_accessor :base_point
     attr_accessor :direction
 
+    def to_s
+      "Line[point#{@base_point.to_element_s}, vector#{@direction.to_element_s}"
+    end
+
     # [Input]
     #  _point_ and _direction_ should be Vector3.
     # [Output]
@@ -21,10 +25,6 @@ module GMath3D
       @direction  = direction
     end
 
-    def to_s
-      "Line[point#{@base_point.to_element_s}, vector#{@direction.to_element_s}"
-    end
-
     # [Input]
     #  _parameter_ should be Numeric.
     # [Output]

data/lib/matrix_util.rb

@@ -0,0 +1,55 @@
+require 'gmath3D'
+require 'matrix'
+
+
+class Matrix
+  def self.from_axis(axis, angle)
+    Util.check_arg_type(Vector3, axis)
+    Util.check_arg_type(Numeric, angle)
+
+    return Matrix[
+           [axis.x*axis.x*(1 - Math.cos(angle)) +        Math.cos(angle),
+                    axis.x*axis.y*(1 - Math.cos(angle)) + axis.z*Math.sin(angle),
+                    axis.x*axis.z*(1 - Math.cos(angle)) - axis.y*Math.sin(angle)],
+           [axis.x*axis.y*(1 - Math.cos(angle)) - axis.z*Math.sin(angle),
+                    axis.y*axis.y*(1 - Math.cos(angle)) +        Math.cos(angle),
+                    axis.y*axis.z*(1 - Math.cos(angle)) + axis.x*Math.sin(angle)],
+           [axis.x*axis.z*(1 - Math.cos(angle)) + axis.y*Math.sin(angle),
+                    axis.y*axis.z*(1 - Math.cos(angle)) - axis.x*Math.sin(angle),
+                    axis.z*axis.z*(1 - Math.cos(angle)) +        Math.cos(angle)]]
+  end
+
+  def self.from_quat(quat)
+    Util.check_arg_type(Quat, quat)
+    qw = quat.w
+    qx = quat.x
+    qy = quat.y
+    qz = quat.z
+
+    x2 = 2.0 * qx * qx;
+    y2 = 2.0 * qy * qy;
+    z2 = 2.0 * qz * qz;
+    xy = 2.0 * qx * qy;
+    yz = 2.0 * qy * qz;
+    zx = 2.0 * qz * qx;
+    wx = 2.0 * qw * qx;
+    wy = 2.0 * qw * qy;
+    wz = 2.0 * qw * qz;
+
+    return Matrix[
+           [ 1.0 - y2 - z2, xy + wz, zx - wy],
+           [ xy - wz, 1.0 - z2 - x2, yz + wx],
+           [ zx + wy, yz - wx, 1.0 - x2 - y2]]
+  end
+
+  alias_method :multi_inner, :*  # hold original multiply processing
+  def multi_new(rhs)
+    if(rhs.kind_of?(Vector3))
+        ans = self.multi_inner(rhs.to_column_vector)
+      return Vector3.new(ans[0,0], ans[1,0], ans[2,0])
+    end
+    multi_inner(rhs)
+  end
+  alias_method :*, :multi_new   # overwrite new multiply processing
+end
+

data/lib/quat.rb

@@ -0,0 +1,303 @@
+require 'gmath3D'
+
+module GMath3D
+  #
+  # Quat represents quaternion.
+  #
+  class Quat
+public
+    attr_accessor :x
+    attr_accessor :y
+    attr_accessor :z
+    attr_accessor :w
+
+    # [Input]
+    #  _x_, _y_, _z_, _w_should be Numeric.
+    # [Output]
+    #  return new instance of Quat.
+    def initialize(x=0.0,y=0.0,z=0.0,w=0.0)
+      Util.check_arg_type(Numeric, x)
+      Util.check_arg_type(Numeric, y)
+      Util.check_arg_type(Numeric, z)
+      Util.check_arg_type(Numeric, w)
+      super()
+      @x = x
+      @y = y
+      @z = z
+      @w = w
+    end
+
+    # [Input]
+    #  _axsi_ should be Vector3 and _angle_ should be Numeric.
+    # [Output]
+    #  return new instance of Quat.
+    def self.from_axis(axis, angle)
+      Util.check_arg_type(Vector3, axis)
+      Util.check_arg_type(Numeric, angle)
+      s = Math.sin(0.5*angle)
+      x = s * axis.x
+      y = s * axis.y
+      z = s * axis.z
+      w = Math.cos(0.5*angle)
+      return Quat.new(x,y,z,w)
+    end
+
+    # [Input]
+    #  _matrix_ should be Matrix which row and column size are 3.
+    # [Output]
+    #  return new instance of Quat.
+    def self.from_matrix(mat)
+      fourWSquaredMinus1 = mat[0,0] + mat[1,1] + mat[2,2]
+      fourXSquaredMinus1 = mat[0,0] - mat[1,1] - mat[2,2]
+      fourYSquaredMinus1 = mat[1,1] - mat[0,0] - mat[2,2]
+      fourZSquaredMinus1 = mat[2,2] - mat[0,0] - mat[1,1]
+
+      biggestIndex = 0
+      fourBiggestSquaredMinus1 = fourWSquaredMinus1
+      if(fourXSquaredMinus1 > fourBiggestSquaredMinus1)
+        fourBiggestSquaredMinus1 = fourXSquaredMinus1
+        biggestIndex = 1
+      end
+      if(fourYSquaredMinus1 > fourBiggestSquaredMinus1)
+        fourBiggestSquaredMinus1 = fourYSquaredMinus1
+        biggestIndex = 2
+      end
+      if(fourZSquaredMinus1 > fourBiggestSquaredMinus1)
+        fourBiggestSquaredMinus1 = fourZSquaredMinus1
+        biggestIndex = 3
+      end
+
+      biggestVal = Math.sqrt(fourBiggestSquaredMinus1 + 1.0) * 0.5
+      multi = 0.25 / biggestVal
+
+      case biggestIndex
+      when 0
+        w = biggestVal
+        x = (mat[1,2] - mat[2,1]) *multi
+        y = (mat[2,0] - mat[0,2]) *multi
+        z = (mat[0,1] - mat[1,0]) *multi
+      when 1
+        x = biggestVal;
+        w = (mat[1,2] - mat[2,1]) *multi
+        y = (mat[0,1] + mat[1,0]) *multi
+        z = (mat[2,0] + mat[0,2]) *multi
+      when 2
+        y = biggestVal;
+        w = (mat[2,0] - mat[0,2]) *multi
+        x = (mat[0,1] + mat[1,0]) *multi
+        z = (mat[1,2] + mat[2,1]) *multi
+      when 3
+        z = biggestVal;
+        w = (mat[0,1] - mat[1,0]) *multi
+        x = (mat[2,0] + mat[0,2]) *multi
+        y = (mat[1,2] + mat[2,1]) *multi
+      end
+      return Quat.new(x,y,z,w)
+    end
+
+    def to_element_s
+      "[#{@x}, #{@y}, #{@z}, #{@w}]"
+    end
+
+    def to_s
+      "Quat" + to_element_s
+    end
+
+    # [Input]
+    #  _rhs_ should be Quat.
+    # [Output]
+    #  return true if rhs equals myself.
+    def ==(rhs)
+      return false if( !rhs.kind_of?(Quat) )
+      return false if(self.x != rhs.x)
+      return false if(self.y != rhs.y)
+      return false if(self.z != rhs.z)
+      return false if(self.w != rhs.w)
+      true
+    end
+
+    # [Output]
+    #  return conjugated Quat.
+    def conjugate
+      return Quat.new( -self.x, -self.y, -self.z, self.w)
+    end
+
+    # [Output]
+    #  return normalized result as Quat.
+    def normalize()
+      mag = Math.sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
+      return Quat.new(self.x/mag, self.y/mag, self.z/mag, self.w/mag)
+    end
+
+    # [Input]
+    #  _rhs_ should be Quat.
+    # [Output]
+    #  return added result as Quat.
+    def +(rhs)
+      Util.check_arg_type(Quat, rhs)
+      t1 = Vector3.new(self.x, self.y, self.z)
+      t2 = Vector3.new(rhs.x, rhs.y, rhs.z)
+      dot = t1.dot(t2)
+      t3 = t2.cross(t1)
+
+      t1 *= rhs.w
+      t2 *= self.w
+
+      tf = t1 + t2 + t3
+      rtn_w = self.w * rhs.w - dot
+
+      return Quat.new(tf.x, tf.y, tf.z, rtn_w).normalize
+    end
+
+    # [Input]
+    #  _rsh_ should be Quat.
+    # [Output]
+    #  return (outer products) multiplyed result as Quat.
+    def *(rhs)
+      Util.check_arg_type(Quat, rhs)
+
+      pw = self.w; px = self.x; py = self.y; pz = self.z;
+      qw = rhs.w ; qx = rhs.x ; qy = rhs.y ; qz = rhs.z;
+
+      w = pw * qw - px * qx - py * qy - pz * qz
+      x = pw * qx + px * qw + py * qz - pz * qy
+      y = pw * qy - px * qz + py * qw + pz * qx
+      z = pw * qz + px * qy - py * qx + pz * qw
+      return Quat.new( x,y,z,w )
+    end
+
+=begin
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return subtracted result as Vector3.
+    def -(rhs)
+      subtract(rhs)
+    end
+
+
+    # [Input]
+    #  _rhs_ should be Numeric.
+    # [Output]
+    #  return divided result as Vector3.
+    def /(rhs)
+      divide(rhs)
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return inner product as Numeric
+    def dot(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      self.x*rhs.x + self.y*rhs.y + self.z*rhs.z
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return cross product as Vector3.
+    def cross(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      Vector3.new(
+              self.y*rhs.z - self.z*rhs.y,
+              self.z*rhs.x - self.x*rhs.z,
+              self.x*rhs.y - self.y*rhs.x)
+    end
+
+    # [Output]
+    #  return vector length as Numeric
+    def length
+      Math::sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return distance between two points as Numeric.
+    def distance(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      Math::sqrt((self.x - rhs.x)*(self.x - rhs.x) + (self.y - rhs.y)*(self.y - rhs.y) + (self.z - rhs.z)*(self.z - rhs.z))
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return two vectors angle as Numeric (rad).
+    def angle(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      vec1Length = self.length ;
+      vec2Length = rhs.length   ;
+      return 0.0 if(vec1Length*vec2Length < self.tolerance )
+      v = self.dot(rhs)/(vec1Length*vec2Length)
+      Math::acos( v )
+    end
+
+    # [Output]
+    #  return normalized vector as Vector3
+    def normalize()
+      self / self.length.to_f
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3
+    # [Output]
+    #  return true if myself and rhs is parallel as boolean
+    def parallel?(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      return false if(self.length < self.tolerance or rhs.length < rhs.tolerance)
+      return false if(self.cross(rhs).length > self.tolerance)
+      return true
+    end
+
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return true if myself and rhs have same direction as boolean.
+    def same_direction?(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      return false if(!parallel?(rhs))
+      return false if(self.dot(rhs) < self.tolerance)
+      return true
+    end
+
+    # This function projects self vector to rhs vector.
+    # [Input]
+    #  _rhs_ should be Vector3.
+    # [Output]
+    #  return projected result as Vector3.
+    def project_to(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      return Vector3.new, 0.0 if( rhs.length < rhs.tolerance )
+      parameter = self.dot( rhs ) / ( rhs.x * rhs.x + rhs.y * rhs.y + rhs.z * rhs.z ).to_f
+      return rhs*parameter, parameter
+    end
+
+    # [Output]
+    #  return column vector as Matrix
+    def to_column_vector
+      return Matrix.column_vector([x,y,z])
+    end
+
+    private
+
+    def add(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      Vector3.new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z)
+    end
+    def subtract(rhs)
+      Util.check_arg_type(Vector3, rhs)
+      Vector3.new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z)
+    end
+    def multiply(rhs)
+      Util.check_arg_type(::Numeric, rhs)
+      Vector3.new(self.x * rhs, self.y * rhs, self.z * rhs)
+    end
+    def divide(rhs)
+      Util.check_arg_type(::Numeric, rhs)
+      Vector3.new(self.x.to_f / rhs, self.y / rhs.to_f, self.z / rhs.to_f)
+    end
+=end
+  end
+end
+

data/lib/triangle.rb

@@ -123,16 +123,24 @@ public
     end
 
     # [Input]
-    #  _target_ shold be Vector3.
+    #  _target_ shold be Vector3 or Line or Plane.
     # [Output]
-    #  return "distance, point on triangle" as [Numeric, Vector3].
+    #  [In case _target_ is Vector3]
+    #   return "distance, point on triangle" as [Numeric, Vector3].
+    #  [In case _target_ is Line]
+    #   return "distance, point on tirangle, point on line, parameter on line" as [Numeric, Vector3, Vector3, Numeric].
+    #  [In case _target_ is Plane]
+    #   return "distance, intersect_line(or closet edge), point_on_triangle, point_on_plane" as [Numeric, Vector3, Vector3, Vector3].
     def distance(target)
       # with Point
       if(target.kind_of?(Vector3))
         return distance_to_point(target)
       elsif(target.kind_of?(Line))
-        #with Line
-#        return distance_to_line(target)
+      #with Line
+        return distance_to_line(target)
+      elsif(target.kind_of?(Plane))
+      #with Plane
+        return distance_to_plane(target)
       end
       Util.raise_argurment_error(target)
     end
@@ -164,6 +172,67 @@ private
       finite_lines = self.edges
       return FiniteLine.ary_distanc_to_point(finite_lines, target_point)
     end
+
+    def distance_to_line(target_line)
+      plane = Plane.new( vertices[0], self.normal )
+      distance, point_on_plane, parameter_on_line = plane.distance( target_line )
+      if( point_on_plane == nil)
+        # parallel case
+        # check distance to FiniteLines
+        finite_lines = self.edges
+        distance, point_on_edge, point_on_target, param_on_finiteline, param_on_target =
+          FiniteLine.ary_distance_to_line(finite_lines, target_line)
+        return distance, nil, nil, nil
+      end
+      if( self.contains?(point_on_plane) )
+        return distance, point_on_plane, point_on_plane, parameter_on_line
+      end
+      # check distance to FiniteLines
+      finite_lines = self.edges
+      distance, point_on_edge, point_on_target, param_on_finiteline, param_on_target =
+        FiniteLine.ary_distance_to_line(finite_lines, target_line)
+      return distance, point_on_edge, point_on_target, param_on_target
+    end
+
+    def distance_to_plane(target_plane)
+      triangle_plane = Plane.new( vertices[0], self.normal )
+      distance, intersect_line_each_plane = triangle_plane.distance( target_plane )
+      if( intersect_line_each_plane == nil )
+        return distance, nil, nil, nil
+      end
+
+      # check distance from intersection and each edge.
+      distance_zero_count = 0
+      distance_info = Array.new(0)
+      prallel_edge_ary = Array.new(0)
+      self.edges.each do |edge|
+        distance, point_on_edge, point_on_line = edge.distance( intersect_line_each_plane)
+        if point_on_edge != nil && point_on_line != nil
+          distance_info.push([distance, point_on_edge, point_on_line])
+          if distance <= self.tolerance
+            distance_zero_count += 1
+          end
+        else
+          prallel_edge_ary.push( edge )
+        end
+      end
+      distance_info.sort!{|a,b| a[0] <=> b[0]}
+      # distance, intersect_line(or closet edge), point_on_triangle, point_on_plan
+      if (distance_zero_count == 2)
+        point1 = distance_info[0][1]
+        point2 = distance_info[1][1]
+        if point1.distance(point2) > self.tolerance
+          return 0.0, FiniteLine.new(point1, point2), nil, nil
+        end
+        return 0.0, nil, point1, point1
+      elsif (distance_zero_count == 0)
+        distance, closest_point_on_plane = target_plane.distance(distance_info[0][1])
+        if(distance_info[0][1] != distance_info[1][1])
+          return distance, FiniteLine.new(distance_info[0][1], distance_info[1][1]), nil, nil
+        end
+        return distance, nil, distance_info[0][1], closest_point_on_plane
+      end
+      return 0.0, nil, nil, nil
+    end
   end
 end
-

data/test/test_finite_line.rb

@@ -24,6 +24,15 @@ class FiniteLineTestCase < MiniTest::Unit::TestCase
     assert_equal("FiniteLine[from[1, 0, 2], to[1, -3.5, 2]]", line.to_s)
   end
 
+  def test_equals
+    assert_equal(FiniteLine.new(Vector3.new(1,2,3), Vector3.new(2,3,4)),
+                FiniteLine.new(Vector3.new(1.0,2.0,3.0), Vector3.new(2.0,3.0,4.0)))
+
+    assert(FiniteLine.new(Vector3.new(1,2,3), Vector3.new(2,3,4)) == FiniteLine.new(Vector3.new(1.0,2.0,3.0), Vector3.new(2.0,3.0,4.0)))
+    assert(FiniteLine.new(Vector3.new(1,2,3), Vector3.new(2,3,3)) != FiniteLine.new(Vector3.new(1.0,2.0,3.0), Vector3.new(2.0,3.0,4.0)))
+    assert(FiniteLine.new(Vector3.new(1,2,3), Vector3.new(2,3,4)) != FiniteLine.new(Vector3.new(2,3,4), Vector3.new(1,2,3)))
+  end
+
   def test_direction
     start_point_tmp = Vector3.new(1.0,  0.0, 2.0)
     end_point_tmp   = Vector3.new(1.0, -3.5, 1.0)

data/test/test_matrix_util.rb

@@ -0,0 +1,84 @@
+# -*- coding: cp932 -*-
+$LOAD_PATH.unshift(File.dirname(__FILE__))
+require 'helper'
+
+include GMath3D
+
+MiniTest::Unit.autorun
+
+class MatrixTestCase < MiniTest::Unit::TestCase
+  def setup
+    @mat_default = Matrix.zero(3)
+    @mat = Matrix[[1,2,3],[4,5,6],[7,8,9]]
+  end
+
+  def test_initalize
+    @mat_default.each do |cell|
+      assert_equal(0, cell)
+    end
+
+    i = 1
+    @mat.each do |cell|
+      assert_equal(i, cell)
+      i+=1
+    end
+  end
+
+  def test_to_s
+    assert_equal("Matrix[[0, 0, 0], [0, 0, 0], [0, 0, 0]]", @mat_default.to_s)
+    assert_equal("Matrix[[1, 2, 3], [4, 5, 6], [7, 8, 9]]", @mat.to_s)
+  end
+
+  def test_multiply
+    mat = Matrix[[3,2,1],[-4,2,1],[3,4,1]]
+    vec = Matrix.column_vector([2,3,4])
+    ans = mat*vec
+
+    assert_equal(ans[0,0], 16)
+    assert_equal(ans[1,0], 2 )
+    assert_equal(ans[2,0], 22)
+
+    vec2 = Vector3.new(2,3,4)
+    ans2 = mat*(vec2)
+    assert_equal(Vector3.new(16,2,22), ans2)
+  end
+
+  def test_from_axis
+    local_point = Vector3.new(1,0,1)
+    angle = 45*Math::PI/180
+    local_cod_x = Vector3.new(Math.cos(angle), Math.sin(angle),0)
+    x_vec = Vector3.new(1,0,0)
+    diff_angle = x_vec.angle(local_cod_x)
+    axis = x_vec.cross(local_cod_x).normalize()
+
+    rotate_matrix = Matrix.from_axis(axis, diff_angle)
+    rotated_point = rotate_matrix.inv * local_point
+    assert_equal(Vector3.new(Math.sqrt(2)/2, Math.sqrt(2)/2, 1), rotated_point)
+
+    local_point = Vector3.new(1,0,0)
+    local_cod_x = Vector3.new(1,1,1).normalize()
+    diff_angle = x_vec.angle(local_cod_x)
+    axis = x_vec.cross(local_cod_x).normalize()
+
+    assert_equal(Math::PI/2, local_cod_x.angle(axis))
+    rotate_matrix = Matrix.from_axis(axis, diff_angle)
+    rotated_point = rotate_matrix.inv()*local_point
+
+    assert_in_delta(rotated_point.x, rotated_point.y, rotated_point.tolerance)
+    assert_in_delta(rotated_point.x, rotated_point.z, rotated_point.tolerance)
+  end
+
+  def test_from_quat
+    axis = Vector3.new(1,2,4).normalize()
+    angle = 46*Math::PI/180
+    quat = Quat.from_axis(axis, angle)
+    mat_expected = Matrix.from_axis(axis, angle)
+    mat_actual = Matrix.from_quat(quat)
+
+    [0,1,2].each do |i|
+      [0,1,2].each do |j|
+        assert_in_delta( mat_expected[i,j], mat_actual[i,j], 1e-8)
+      end
+    end
+  end
+end

data/test/test_quat.rb

@@ -0,0 +1,135 @@
+$LOAD_PATH.unshift(File.dirname(__FILE__))
+require 'helper'
+
+include GMath3D
+
+MiniTest::Unit.autorun
+
+class QuatTestCase < MiniTest::Unit::TestCase
+  def setup
+    @quat_default = Quat.new()
+    @quat1 = Quat.new(2.0, 3.0, 4.0, 1.0)
+    @quat2 = Quat.new(6.0, 7.0, 8.0, 5.0)
+  end
+
+  def test_initalize
+    assert_equal(0, @quat_default.x)
+    assert_equal(0, @quat_default.y)
+    assert_equal(0, @quat_default.z)
+    assert_equal(0, @quat_default.w)
+
+    assert_equal(2.0, @quat1.x)
+    assert_equal(3.0, @quat1.y)
+    assert_equal(4.0, @quat1.z)
+    assert_equal(1.0, @quat1.w)
+
+    assert_raises ArgumentError do
+      invalidResult = Quat.new( "hoge" )
+    end
+  end
+
+  def test_from_axis
+    axis = Vector3.new(1.0, 0.0, 0.0)
+    rad = 30.0*Math::PI / 180
+    rot_quat = Quat.from_axis(axis, rad)
+    assert_equal( Math.cos(rad/2.0), rot_quat.w)
+    assert_equal( Math.sin(rad/2.0), rot_quat.x)
+    assert_equal( 0.0, rot_quat.y)
+    assert_equal( 0.0, rot_quat.z)
+  end
+
+  def test_from_matrix
+    axis = Vector3.new(1,2,4).normalize()
+    angle = 46*Math::PI/180
+    mat = Matrix.from_axis(axis, angle)
+    quat_expected = Quat.from_axis(axis, angle)
+    quat_actual   = Quat.from_matrix(mat)
+    assert_equal(quat_expected, quat_actual)
+  end
+
+  def test_to_s
+    assert_equal("Quat[2.0, 3.0, 4.0, 1.0]", @quat1.to_s)
+  end
+
+  def test_assign_value
+    assert_equal(2.0, @quat1.x)
+    assert_equal(3.0, @quat1.y)
+    assert_equal(4.0, @quat1.z)
+    assert_equal(1.0, @quat1.w)
+
+    @quat1.x = 2.0
+    @quat1.y *= 2.0
+    @quat1.z -= 3.0
+    @quat1.w /= 2.0
+
+    assert_equal(2.0, @quat1.x)
+    assert_equal(6.0, @quat1.y)
+    assert_equal(1.0, @quat1.z)
+    assert_equal(0.5, @quat1.w)
+  end
+
+  def test_equals
+    assert(!(@quat_default == @quat1))
+    assert(@quat_default != @quat1)
+
+    assert(@quat1 == @quat1)
+
+    quat = Quat.new(2,3,4,1)
+    assert(@quat1 == quat)
+
+    #invlid value comparison
+    assert(@quat1 != "string")
+    assert(@quat1 != -4)
+  end
+
+  def test_conjugate
+    quat1_conjugate = @quat1.conjugate()
+    quat2_conjugate = @quat2.conjugate()
+
+    assert_equal(-2.0, quat1_conjugate.x)
+    assert_equal(-3.0, quat1_conjugate.y)
+    assert_equal(-4.0, quat1_conjugate.z)
+    assert_equal( 1.0, quat1_conjugate.w)
+    assert_equal(-6.0, quat2_conjugate.x)
+    assert_equal(-7.0, quat2_conjugate.y)
+    assert_equal(-8.0, quat2_conjugate.z)
+    assert_equal( 5.0, quat2_conjugate.w)
+  end
+
+  def test_add
+    # implemented, but how to validate?
+  end
+
+  def test_add_invalid_value
+    assert_raises ArgumentError do
+      invalidResult = @quat1 + 5
+    end
+    assert_raises ArgumentError do
+      invalidResult = @quat1 + nil
+    end
+  end
+
+  def test_multiply
+    multiply = @quat1 * @quat2
+    assert_equal(-60, multiply.w)
+    assert_equal( 12, multiply.x)
+    assert_equal( 30, multiply.y)
+    assert_equal( 24, multiply.z)
+
+    @quat1 *= @quat2
+    assert_equal(-60, @quat1.w)
+    assert_equal( 12, @quat1.x)
+    assert_equal( 30, @quat1.y)
+    assert_equal( 24, @quat1.z)
+  end
+
+  def test_multiply_invalid_value
+    assert_raises ArgumentError do
+      invalidResult = @quat1 * 3
+    end
+    assert_raises ArgumentError do
+      invalidResult = @quat2 * nil
+    end
+  end
+
+end

data/test/test_triangle.rb

@@ -147,4 +147,110 @@ class TriangleTestCase < MiniTest::Unit::TestCase
     assert_in_delta(0, distance, @triangle_default.tolerance)
     assert_equal(@triangle_default.center, point_on_triangle)
   end
+
+  def test_distance_to_line
+    # on inside
+    line = Line.new(Vector3.new(0,3,-1), Vector3.new(0,0,4))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(0, distance)
+    assert_equal(Vector3.new(0,3,1), point_on_triangle)
+    assert_equal(Vector3.new(0,3,1), point_on_line)
+    assert_in_delta(0.5, param_on_line, line.tolerance)
+
+    # on edge
+    line = Line.new(Vector3.new(1,3,-1), Vector3.new(0,0,4))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(0, distance)
+    assert_equal(Vector3.new(1,3,2), point_on_triangle)
+    assert_equal(Vector3.new(1,3,2), point_on_line)
+    assert_in_delta(0.75, param_on_line, line.tolerance)
+
+    # on vertex
+    line = Line.new(Vector3.new(-1,3,0), Vector3.new(1,1,2))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(0, distance)
+    assert_equal(Vector3.new(-1,3,0), point_on_triangle)
+    assert_equal(Vector3.new(-1,3,0), point_on_line)
+    assert_in_delta(0.0, param_on_line, line.tolerance)
+
+    # closest point is out of triangle1
+    line = Line.new(Vector3.new(2,3,0), Vector3.new(0,0,4))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(1, distance)
+    assert_equal(Vector3.new(1,3,2), point_on_triangle)
+    assert_equal(Vector3.new(2,3,2), point_on_line)
+    assert_in_delta(0.5, param_on_line, line.tolerance)
+
+    # closest point is out of triangle2
+    line = Line.new(Vector3.new(-2,3,-1), Vector3.new(0,0,1))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(1, distance)
+    assert_equal(Vector3.new(-1,3,0), point_on_triangle)
+    assert_equal(Vector3.new(-2,3,0), point_on_line)
+    assert_in_delta(1, param_on_line, line.tolerance)
+
+    # parallel case
+    line = Line.new(Vector3.new(1,0,4), Vector3.new(0,6,0))
+    distance, point_on_triangle, point_on_line, param_on_line = @triangle.distance(line)
+    assert_equal(2, distance)
+    assert_equal(nil, point_on_triangle)
+    assert_equal(nil, point_on_line)
+    assert_equal(nil, param_on_line)
+  end
+
+  def test_distance_to_plane
+    # intersect case
+    plane = Plane.new(Vector3.new(0,0,1), Vector3.new(1,0,0))
+    distance, intersect_line, point_on_triangle, point_on_plane = @triangle.distance(plane)
+    assert_equal( 0, distance )
+    point1 = Vector3.new(0,2.5,1)
+    point2 = Vector3.new(0,3.5,1)
+    assert( FiniteLine.new(point1, point2) == intersect_line || FiniteLine.new(point2, point1) == intersect_line)
+    assert_equal( nil, point_on_triangle )
+    assert_equal( nil, point_on_plane )
+
+    # contains edge
+    plane = Plane.new(Vector3.new(1,0,1), Vector3.new(-1,0,0))
+    distance, intersect_line, point_on_triangle, point_on_plane = @triangle.distance(plane)
+    assert_equal( 0, distance )
+    point1 = Vector3.new(1,2,2)
+    point2 = Vector3.new(1,4,2)
+    assert( FiniteLine.new(point1, point2) == intersect_line || FiniteLine.new(point2, point1) == intersect_line)
+    assert_equal( nil, point_on_triangle )
+    assert_equal( nil, point_on_plane )
+
+    # contains vertex
+    plane = Plane.new(Vector3.new(-1,0,1), Vector3.new(1,0,0))
+    distance, intersect_line, point_on_triangle, point_on_plane = @triangle.distance(plane)
+    assert_equal( 0, distance )
+    assert_equal( nil, intersect_line)
+    assert_equal( Vector3.new(-1,3,0), point_on_triangle )
+    assert_equal( Vector3.new(-1,3,0), point_on_plane )
+
+    # closeing point is outside of triangle
+    plane = Plane.new(Vector3.new(-2,0,0), Vector3.new(-1,0,0))
+    distance, intersect_line, point_on_triangle, point_on_plane = @triangle.distance(plane)
+    assert_equal( 1, distance )
+    assert_equal( nil, intersect_line)
+    assert_equal( Vector3.new(-1,3,0), point_on_triangle )
+    assert_equal( Vector3.new(-2,3,0), point_on_plane )
+
+    # parallel to plane
+    plane = Plane.new(Vector3.new(0,0,4), Vector3.new(0,0,1))
+    distance, intersect_line, point_on_triangle, point_on_plane = @triangle_default.distance(plane)
+    assert_equal( 4, distance )
+    assert_equal( nil, intersect_line)
+    assert_equal( nil, point_on_triangle )
+    assert_equal( nil, point_on_plane )
+
+    # parallel to edge
+    plane = Plane.new(Vector3.new(3.5,0,3), Vector3.new(-1,0,0))
+    distance, closest_line, point_on_triangle, point_on_plane = @triangle.distance(plane)
+    assert_equal( 2.5, distance )
+    point1 = Vector3.new(1,2,2)
+    point2 = Vector3.new(1,4,2)
+    assert( FiniteLine.new(point1, point2) == closest_line || FiniteLine.new(point2, point1) == closest_line)
+    assert_equal( nil, point_on_triangle )
+    assert_equal( nil, point_on_plane )
+  end
 end

data/test/test_vector3.rb

@@ -72,6 +72,8 @@ class Vector3TestCase < MiniTest::Unit::TestCase
     vector = Vector3.new(1.0 - floatingError2, 2.0 + floatingError2, 3.0)
     assert(@vector != vector)
 
+    assert_equal(Vector3.new(1,2,3), Vector3.new(1.0,2.0,3.0))
+
     #invlid value comparison
     assert(@vector != "string")
     assert(@vector != -4)

metadata

@@ -1,7 +1,7 @@
 --- !ruby/object:Gem::Specification
 name: gmath3D
 version: !ruby/object:Gem::Version
-  version: 0.2.0
+  version: 0.2.1
   prerelease: 
 platform: ruby
 authors:
@@ -9,12 +9,12 @@ authors:
 autorequire: 
 bindir: bin
 cert_chain: []
-date: 2011-10-09 00:00:00.000000000 +09:00
+date: 2011-10-19 00:00:00.000000000 +09:00
 default_executable: 
 dependencies:
 - !ruby/object:Gem::Dependency
   name: bundler
-  requirement: &2176871500 !ruby/object:Gem::Requirement
+  requirement: &2156099140 !ruby/object:Gem::Requirement
     none: false
     requirements:
     - - ~>
@@ -22,10 +22,10 @@ dependencies:
         version: 1.0.0
   type: :development
   prerelease: false
-  version_requirements: *2176871500
+  version_requirements: *2156099140
 - !ruby/object:Gem::Dependency
   name: jeweler
-  requirement: &2176871020 !ruby/object:Gem::Requirement
+  requirement: &2156097640 !ruby/object:Gem::Requirement
     none: false
     requirements:
     - - ~>
@@ -33,7 +33,7 @@ dependencies:
         version: 1.6.4
   type: :development
   prerelease: false
-  version_requirements: *2176871020
+  version_requirements: *2156097640
 description: This library defines 3D geometric elements(point, line, plane etc..).
   It can get two(or more) elements relation, like distance between two elements.
 email: toshi0328@gmail.com
@@ -56,7 +56,9 @@ files:
 - lib/geom.rb
 - lib/gmath3D.rb
 - lib/line.rb
+- lib/matrix_util.rb
 - lib/plane.rb
+- lib/quat.rb
 - lib/rectangle.rb
 - lib/triangle.rb
 - lib/util.rb
@@ -65,9 +67,10 @@ files:
 - test/test_box.rb
 - test/test_finite_line.rb
 - test/test_geom.rb
-- test/test_gmath3D.rb
 - test/test_line.rb
+- test/test_matrix_util.rb
 - test/test_plane.rb
+- test/test_quat.rb
 - test/test_rectangle.rb
 - test/test_triangle.rb
 - test/test_util.rb
@@ -88,7 +91,7 @@ required_ruby_version: !ruby/object:Gem::Requirement
       version: '0'
       segments:
       - 0
-      hash: 3189821684836837166
+      hash: 3561879194129992333
 required_rubygems_version: !ruby/object:Gem::Requirement
   none: false
   requirements:

data/test/test_gmath3D.rb

@@ -1,15 +0,0 @@
-$LOAD_PATH.unshift(File.dirname(__FILE__))
-
-require 'helper'
-require 'test_geom'
-require 'test_util'
-require 'test_vector3'
-require 'test_line'
-require 'test_finite_line'
-require 'test_plane'
-require 'test_rectangle'
-require 'test_triangle'
-require 'test_box'
-
-
-