bullet3 0.1.0

data/lib/bullet3/dynamics.rb

@@ -0,0 +1,4 @@
+# frozen_string_literal: true
+
+module Bullet3
+end

data/lib/bullet3/io.rb

@@ -0,0 +1,110 @@
+# frozen_string_literal: true
+
+module Bullet3
+  module IO
+    class Serializer
+      def initialize(simulation = nil)
+        @simulation = simulation
+      end
+
+      def serialize_world(simulation_or_world = nil)
+        target = simulation_or_world || @simulation
+        raise ArgumentError, "a simulation or dynamics world is required" unless target
+
+        if native_serializer
+          return native_serializer.serialize_world(extract_world(target))
+        end
+
+        raise Bullet3::Error, "native extension is required for .bullet serialization"
+      end
+
+      def save_world(simulation_or_world, filename)
+        if bullet_file?(filename)
+          return save_bullet(simulation_or_world, filename)
+        end
+
+        simulation = extract_simulation(simulation_or_world)
+        simulation.save_world(filename)
+      end
+
+      def load_world(simulation, filename)
+        if bullet_file?(filename)
+          return load_bullet(simulation, filename)
+        end
+
+        extract_simulation(simulation).load_world(filename)
+      end
+
+      def save_bullet(simulation_or_world, filename)
+        raise Bullet3::Error, "native extension is required for .bullet serialization" unless native_serializer
+
+        native_serializer.save_world(extract_world(simulation_or_world), filename)
+      end
+
+      def load_bullet(simulation, filename)
+        extract_simulation(simulation).load_bullet(filename)
+      end
+
+      private
+
+      def native_serializer
+        return nil unless IO.const_defined?(:DefaultSerializer, false)
+
+        @native_serializer ||= DefaultSerializer.new
+      end
+
+      def extract_simulation(value)
+        return value if value.is_a?(Simulation)
+
+        raise ArgumentError, "expected Bullet3::Simulation"
+      end
+
+      def extract_world(value)
+        return value.world if value.is_a?(Simulation)
+        return value if value.is_a?(DiscreteDynamicsWorld)
+
+        raise ArgumentError, "expected Bullet3::Simulation or Bullet3::DiscreteDynamicsWorld"
+      end
+
+      def bullet_file?(filename)
+        File.extname(filename.to_s) == ".bullet"
+      end
+    end
+
+    class URDFImporter
+      attr_reader :simulation
+
+      def initialize(simulation = Simulation.new)
+        @simulation = simulation
+      end
+
+      def load(filename, **options)
+        simulation.load_urdf(filename, **options)
+      end
+    end
+
+    class SDFImporter
+      attr_reader :simulation
+
+      def initialize(simulation = Simulation.new)
+        @simulation = simulation
+      end
+
+      def load(filename, **options)
+        simulation.load_sdf(filename, **options)
+      end
+    end
+
+    class MJCFImporter
+      attr_reader :simulation
+
+      def initialize(simulation = Simulation.new)
+        @simulation = simulation
+      end
+
+      def load(filename, **options)
+        simulation.load_mjcf(filename, **options)
+      end
+    end
+  end
+end

data/lib/bullet3/linear_math/matrix3x3.rb

@@ -0,0 +1,69 @@
+# frozen_string_literal: true
+
+module Bullet3
+  class Matrix3x3
+    attr_reader :rows
+
+    def self.identity
+      new([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
+    end
+
+    def self.from_quaternion(quaternion)
+      q = Quaternion.coerce(quaternion).normalized
+      xx = q.x * q.x
+      yy = q.y * q.y
+      zz = q.z * q.z
+      xy = q.x * q.y
+      xz = q.x * q.z
+      yz = q.y * q.z
+      wx = q.w * q.x
+      wy = q.w * q.y
+      wz = q.w * q.z
+
+      new([
+        [1.0 - (2.0 * (yy + zz)), 2.0 * (xy - wz), 2.0 * (xz + wy)],
+        [2.0 * (xy + wz), 1.0 - (2.0 * (xx + zz)), 2.0 * (yz - wx)],
+        [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - (2.0 * (xx + yy))]
+      ])
+    end
+
+    def initialize(*values)
+      rows = if values.empty?
+               self.class.identity.rows
+             elsif values.length == 1
+               values.first
+             elsif values.length == 9
+               values.each_slice(3).to_a
+             else
+               raise ArgumentError, "expected a 3x3 row array or 9 scalar values"
+             end
+
+      @rows = rows.map { |row| row.map { |value| Float(value) } }
+      raise ArgumentError, "matrix must be 3x3" unless @rows.length == 3 && @rows.all? { |row| row.length == 3 }
+    end
+
+    def *(vector)
+      vector = Vector3.coerce(vector)
+      Vector3.new(
+        dot_row(0, vector),
+        dot_row(1, vector),
+        dot_row(2, vector)
+      )
+    end
+
+    def transpose
+      self.class.new(rows.transpose)
+    end
+
+    def to_a
+      rows.map(&:dup)
+    end
+
+    private
+
+    def dot_row(index, vector)
+      row = rows[index]
+      (row[0] * vector.x) + (row[1] * vector.y) + (row[2] * vector.z)
+    end
+  end unless const_defined?(:Matrix3x3, false)
+end

data/lib/bullet3/linear_math/quaternion.rb

@@ -0,0 +1,187 @@
+# frozen_string_literal: true
+
+module Bullet3
+  class Quaternion
+    EPSILON = 1e-6
+
+    attr_accessor :x, :y, :z, :w
+
+    def self.identity
+      new(0.0, 0.0, 0.0, 1.0)
+    end
+
+    def self.from_axis_angle(axis, angle)
+      axis = Vector3.coerce(axis).normalized
+      half_angle = Float(angle) / 2.0
+      scale = Math.sin(half_angle)
+
+      new(axis.x * scale, axis.y * scale, axis.z * scale, Math.cos(half_angle))
+    end
+
+    def self.from_euler(roll, pitch, yaw)
+      cr = Math.cos(Float(roll) / 2.0)
+      sr = Math.sin(Float(roll) / 2.0)
+      cp = Math.cos(Float(pitch) / 2.0)
+      sp = Math.sin(Float(pitch) / 2.0)
+      cy = Math.cos(Float(yaw) / 2.0)
+      sy = Math.sin(Float(yaw) / 2.0)
+
+      new(
+        (sr * cp * cy) - (cr * sp * sy),
+        (cr * sp * cy) + (sr * cp * sy),
+        (cr * cp * sy) - (sr * sp * cy),
+        (cr * cp * cy) + (sr * sp * sy)
+      )
+    end
+
+    def initialize(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
+      @x = Float(x)
+      @y = Float(y)
+      @z = Float(z)
+      @w = Float(w)
+    end
+
+    def *(other)
+      other = self.class.coerce(other)
+      self.class.new(
+        (w * other.x) + (x * other.w) + (y * other.z) - (z * other.y),
+        (w * other.y) - (x * other.z) + (y * other.w) + (z * other.x),
+        (w * other.z) + (x * other.y) - (y * other.x) + (z * other.w),
+        (w * other.w) - (x * other.x) - (y * other.y) - (z * other.z)
+      )
+    end
+
+    def ==(other)
+      other = self.class.try_coerce(other)
+      return false unless other
+
+      to_a.zip(other.to_a).all? { |left, right| (left - right).abs <= EPSILON }
+    end
+
+    def inverse
+      magnitude = length2
+      raise ZeroDivisionError, "cannot invert a zero-length quaternion" if magnitude <= EPSILON
+
+      self.class.new(-x / magnitude, -y / magnitude, -z / magnitude, w / magnitude)
+    end
+
+    def angle
+      2.0 * Math.acos(w.clamp(-1.0, 1.0))
+    end
+
+    def axis
+      scale = Math.sqrt(1.0 - (w * w))
+      return Vector3.new(1.0, 0.0, 0.0) if scale <= EPSILON
+
+      Vector3.new(x / scale, y / scale, z / scale)
+    end
+
+    def slerp(other, t)
+      other = self.class.coerce(other)
+      t = Float(t)
+      cosine = dot(other)
+
+      if cosine.negative?
+        other = self.class.new(-other.x, -other.y, -other.z, -other.w)
+        cosine = -cosine
+      end
+
+      return linear_interpolate(other, t).normalized if cosine > 0.9995
+
+      theta = Math.acos(cosine.clamp(-1.0, 1.0))
+      sin_theta = Math.sin(theta)
+      left = Math.sin((1.0 - t) * theta) / sin_theta
+      right = Math.sin(t * theta) / sin_theta
+
+      self.class.new(
+        (x * left) + (other.x * right),
+        (y * left) + (other.y * right),
+        (z * left) + (other.z * right),
+        (w * left) + (other.w * right)
+      )
+    end
+
+    def to_euler
+      roll = Math.atan2(2.0 * ((w * x) + (y * z)), 1.0 - (2.0 * ((x * x) + (y * y))))
+      pitch_sin = 2.0 * ((w * y) - (z * x))
+      pitch = if pitch_sin.abs >= 1.0
+                pitch_sin.negative? ? -Math::PI / 2.0 : Math::PI / 2.0
+              else
+                Math.asin(pitch_sin)
+              end
+      yaw = Math.atan2(2.0 * ((w * z) + (x * y)), 1.0 - (2.0 * ((y * y) + (z * z))))
+
+      [roll, pitch, yaw]
+    end
+
+    def to_matrix
+      Matrix3x3.from_quaternion(self)
+    end
+
+    def normalized
+      magnitude = Math.sqrt(length2)
+      return self.class.identity if magnitude <= EPSILON
+
+      self.class.new(x / magnitude, y / magnitude, z / magnitude, w / magnitude)
+    end
+
+    def to_a
+      [x, y, z, w]
+    end
+
+    def self.coerce(value)
+      coerced = try_coerce(value)
+      return coerced if coerced
+
+      raise TypeError, "expected Bullet3::Quaternion or a 4-element Array"
+    end
+
+    def self.try_coerce(value)
+      return value if value.is_a?(self)
+
+      return new(*value) if value.respond_to?(:to_ary) && value.to_ary.length == 4
+
+      nil
+    end
+
+    protected
+
+    def dot(other)
+      (x * other.x) + (y * other.y) + (z * other.z) + (w * other.w)
+    end
+
+    private
+
+    def length2
+      dot(self)
+    end
+
+    def linear_interpolate(other, t)
+      self.class.new(
+        x + ((other.x - x) * t),
+        y + ((other.y - y) * t),
+        z + ((other.z - z) * t),
+        w + ((other.w - w) * t)
+      )
+    end
+  end unless const_defined?(:Quaternion, false)
+
+  class Quaternion
+    EPSILON = 1e-6 unless const_defined?(:EPSILON, false)
+
+    def self.coerce(value)
+      coerced = try_coerce(value)
+      return coerced if coerced
+
+      raise TypeError, "expected Bullet3::Quaternion or a 4-element Array"
+    end
+
+    def self.try_coerce(value)
+      return value if value.is_a?(self)
+
+      return new(*value) if value.respond_to?(:to_ary) && value.to_ary.length == 4
+
+      nil
+    end
+  end
+end

data/lib/bullet3/linear_math/transform.rb

@@ -0,0 +1,48 @@
+# frozen_string_literal: true
+
+module Bullet3
+  class Transform
+    attr_accessor :origin, :rotation
+
+    def self.identity
+      new
+    end
+
+    def initialize(rotation = Quaternion.identity, origin = Vector3.zero)
+      @rotation = Quaternion.coerce(rotation)
+      @origin = Vector3.coerce(origin)
+    end
+
+    def basis
+      rotation.to_matrix
+    end
+
+    def *(other)
+      case other
+      when Transform
+        self.class.new(rotation * other.rotation, transform_vector(other.origin))
+      else
+        transform_vector(other)
+      end
+    end
+
+    def inverse
+      inverse_rotation = rotation.inverse.normalized
+      self.class.new(inverse_rotation, inverse_rotation.to_matrix * -origin)
+    end
+
+    def inverse_times(other)
+      inverse * other
+    end
+
+    def to_a
+      [origin.to_a, rotation.to_a]
+    end
+
+    private
+
+    def transform_vector(vector)
+      (basis * vector) + origin
+    end
+  end unless const_defined?(:Transform, false)
+end

data/lib/bullet3/linear_math/vector3.rb

@@ -0,0 +1,199 @@
+# frozen_string_literal: true
+
+module Bullet3
+  class Vector3
+    include Enumerable
+
+    EPSILON = 1e-6
+    COMPONENTS = { 0 => :x, 1 => :y, 2 => :z }.freeze
+
+    attr_accessor :x, :y, :z
+
+    def self.zero
+      new
+    end
+
+    def initialize(x = 0.0, y = 0.0, z = 0.0)
+      @x = Float(x)
+      @y = Float(y)
+      @z = Float(z)
+    end
+
+    def [](index)
+      public_send(component_name(index))
+    end
+
+    def []=(index, value)
+      public_send("#{component_name(index)}=", Float(value))
+    end
+
+    def each(&block)
+      to_a.each(&block)
+    end
+
+    def +(other)
+      other = self.class.coerce(other)
+      self.class.new(x + other.x, y + other.y, z + other.z)
+    end
+
+    def -(other)
+      other = self.class.coerce(other)
+      self.class.new(x - other.x, y - other.y, z - other.z)
+    end
+
+    def -@
+      self.class.new(-x, -y, -z)
+    end
+
+    def *(scalar)
+      self.class.new(x * Float(scalar), y * Float(scalar), z * Float(scalar))
+    end
+
+    def /(scalar)
+      scalar = Float(scalar)
+      raise ZeroDivisionError, "divided by 0" if scalar.zero?
+
+      self.class.new(x / scalar, y / scalar, z / scalar)
+    end
+
+    def ==(other)
+      other = self.class.try_coerce(other)
+      return false unless other
+
+      distance2(other) <= EPSILON * EPSILON
+    end
+
+    def length
+      Math.sqrt(length2)
+    end
+
+    def length2
+      dot(self)
+    end
+
+    def normalize
+      normalized = self.normalized
+      self.x = normalized.x
+      self.y = normalized.y
+      self.z = normalized.z
+      self
+    end
+
+    alias normalize! normalize
+
+    def normalized
+      magnitude = length
+      return self.class.zero if magnitude <= EPSILON
+
+      self / magnitude
+    end
+
+    def dot(other)
+      other = self.class.coerce(other)
+      (x * other.x) + (y * other.y) + (z * other.z)
+    end
+
+    def cross(other)
+      other = self.class.coerce(other)
+      self.class.new(
+        (y * other.z) - (z * other.y),
+        (z * other.x) - (x * other.z),
+        (x * other.y) - (y * other.x)
+      )
+    end
+
+    def distance(other)
+      Math.sqrt(distance2(other))
+    end
+
+    def distance2(other)
+      (self - other).length2
+    end
+
+    def lerp(other, t)
+      other = self.class.coerce(other)
+      self + ((other - self) * Float(t))
+    end
+
+    def angle(other)
+      other = self.class.coerce(other)
+      denominator = length * other.length
+      return 0.0 if denominator <= EPSILON
+
+      cosine = dot(other) / denominator
+      Math.acos(cosine.clamp(-1.0, 1.0))
+    end
+
+    def absolute
+      self.class.new(x.abs, y.abs, z.abs)
+    end
+
+    def to_a
+      [x, y, z]
+    end
+
+    def to_s
+      "(#{x}, #{y}, #{z})"
+    end
+
+    def inspect
+      "#<Bullet3::Vector3 (#{x}, #{y}, #{z})>"
+    end
+
+    def self.coerce(value)
+      coerced = try_coerce(value)
+      return coerced if coerced
+
+      raise TypeError, "expected Bullet3::Vector3 or a 3-element Array"
+    end
+
+    def self.try_coerce(value)
+      return value if value.is_a?(self)
+
+      return new(*value) if value.respond_to?(:to_ary) && value.to_ary.length == 3
+
+      nil
+    end
+
+    private
+
+    def component_name(index)
+      COMPONENTS.fetch(Integer(index))
+    rescue KeyError
+      raise IndexError, "index #{index} outside vector"
+    end
+  end unless const_defined?(:Vector3, false)
+
+  class Vector3
+    EPSILON = 1e-6 unless const_defined?(:EPSILON, false)
+
+    include Enumerable unless include?(Enumerable)
+
+    def each(&block)
+      to_a.each(&block)
+    end
+
+    def to_s
+      "(#{x}, #{y}, #{z})"
+    end
+
+    def inspect
+      "#<Bullet3::Vector3 (#{x}, #{y}, #{z})>"
+    end
+
+    def self.coerce(value)
+      coerced = try_coerce(value)
+      return coerced if coerced
+
+      raise TypeError, "expected Bullet3::Vector3 or a 3-element Array"
+    end
+
+    def self.try_coerce(value)
+      return value if value.is_a?(self)
+
+      return new(*value) if value.respond_to?(:to_ary) && value.to_ary.length == 3
+
+      nil
+    end
+  end
+end

data/lib/bullet3/linear_math.rb

@@ -0,0 +1,6 @@
+# frozen_string_literal: true
+
+require_relative "linear_math/vector3"
+require_relative "linear_math/quaternion"
+require_relative "linear_math/matrix3x3"
+require_relative "linear_math/transform"

data/lib/bullet3/multi_body.rb

@@ -0,0 +1,6 @@
+# frozen_string_literal: true
+
+module Bullet3
+  module MultiBody
+  end unless const_defined?(:MultiBody, false)
+end