3rb 0.1.0

data/lib/3rb/math/quaternion.rb

@@ -0,0 +1,337 @@
+# frozen_string_literal: true
+
+module Sunrb
+  class Quaternion
+    attr_accessor :x, :y, :z, :w
+
+    def initialize(x = 0, y = 0, z = 0, w = 1)
+      @x = x.to_f
+      @y = y.to_f
+      @z = z.to_f
+      @w = w.to_f
+    end
+
+    def set(x, y, z, w)
+      @x = x.to_f
+      @y = y.to_f
+      @z = z.to_f
+      @w = w.to_f
+      self
+    end
+
+    def copy(q)
+      @x = q.x
+      @y = q.y
+      @z = q.z
+      @w = q.w
+      self
+    end
+
+    def clone
+      Quaternion.new(@x, @y, @z, @w)
+    end
+
+    def identity
+      set(0, 0, 0, 1)
+    end
+
+    def *(other)
+      clone.multiply!(other)
+    end
+
+    def multiply!(other)
+      qax = @x
+      qay = @y
+      qaz = @z
+      qaw = @w
+      qbx = other.x
+      qby = other.y
+      qbz = other.z
+      qbw = other.w
+
+      @x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby
+      @y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz
+      @z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx
+      @w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz
+
+      self
+    end
+
+    def premultiply!(other)
+      tmp = other * self
+      copy(tmp)
+    end
+
+    def dot(other)
+      @x * other.x + @y * other.y + @z * other.z + @w * other.w
+    end
+
+    def length_sq
+      @x * @x + @y * @y + @z * @z + @w * @w
+    end
+
+    def length
+      Math.sqrt(length_sq)
+    end
+
+    def normalize
+      clone.normalize!
+    end
+
+    def normalize!
+      len = length
+      return identity if len.zero?
+
+      len = 1.0 / len
+      @x *= len
+      @y *= len
+      @z *= len
+      @w *= len
+      self
+    end
+
+    def conjugate
+      clone.conjugate!
+    end
+
+    def conjugate!
+      @x = -@x
+      @y = -@y
+      @z = -@z
+      self
+    end
+
+    def invert
+      clone.invert!
+    end
+
+    def invert!
+      conjugate!.normalize!
+    end
+
+    def slerp(other, t)
+      return clone if t.zero?
+      return other.clone if t == 1
+
+      x = @x
+      y = @y
+      z = @z
+      w = @w
+
+      cos_half_theta = w * other.w + x * other.x + y * other.y + z * other.z
+
+      if cos_half_theta < 0
+        @w = -other.w
+        @x = -other.x
+        @y = -other.y
+        @z = -other.z
+        cos_half_theta = -cos_half_theta
+      else
+        copy(other)
+      end
+
+      if cos_half_theta >= 1.0
+        @w = w
+        @x = x
+        @y = y
+        @z = z
+        return self
+      end
+
+      sqr_sin_half_theta = 1.0 - cos_half_theta * cos_half_theta
+
+      if sqr_sin_half_theta <= Float::EPSILON
+        s = 1 - t
+        @w = s * w + t * @w
+        @x = s * x + t * @x
+        @y = s * y + t * @y
+        @z = s * z + t * @z
+        normalize!
+        return self
+      end
+
+      sin_half_theta = Math.sqrt(sqr_sin_half_theta)
+      half_theta = Math.atan2(sin_half_theta, cos_half_theta)
+      ratio_a = Math.sin((1 - t) * half_theta) / sin_half_theta
+      ratio_b = Math.sin(t * half_theta) / sin_half_theta
+
+      @w = w * ratio_a + @w * ratio_b
+      @x = x * ratio_a + @x * ratio_b
+      @y = y * ratio_a + @y * ratio_b
+      @z = z * ratio_a + @z * ratio_b
+
+      self
+    end
+
+    def set_from_axis_angle(axis, angle)
+      half_angle = angle / 2.0
+      s = Math.sin(half_angle)
+
+      @x = axis.x * s
+      @y = axis.y * s
+      @z = axis.z * s
+      @w = Math.cos(half_angle)
+
+      self
+    end
+
+    def set_from_euler(euler, update = true)
+      x = euler.x
+      y = euler.y
+      z = euler.z
+      order = euler.order
+
+      c1 = Math.cos(x / 2)
+      c2 = Math.cos(y / 2)
+      c3 = Math.cos(z / 2)
+
+      s1 = Math.sin(x / 2)
+      s2 = Math.sin(y / 2)
+      s3 = Math.sin(z / 2)
+
+      case order
+      when "XYZ"
+        @x = s1 * c2 * c3 + c1 * s2 * s3
+        @y = c1 * s2 * c3 - s1 * c2 * s3
+        @z = c1 * c2 * s3 + s1 * s2 * c3
+        @w = c1 * c2 * c3 - s1 * s2 * s3
+      when "YXZ"
+        @x = s1 * c2 * c3 + c1 * s2 * s3
+        @y = c1 * s2 * c3 - s1 * c2 * s3
+        @z = c1 * c2 * s3 - s1 * s2 * c3
+        @w = c1 * c2 * c3 + s1 * s2 * s3
+      when "ZXY"
+        @x = s1 * c2 * c3 - c1 * s2 * s3
+        @y = c1 * s2 * c3 + s1 * c2 * s3
+        @z = c1 * c2 * s3 + s1 * s2 * c3
+        @w = c1 * c2 * c3 - s1 * s2 * s3
+      when "ZYX"
+        @x = s1 * c2 * c3 - c1 * s2 * s3
+        @y = c1 * s2 * c3 + s1 * c2 * s3
+        @z = c1 * c2 * s3 - s1 * s2 * c3
+        @w = c1 * c2 * c3 + s1 * s2 * s3
+      when "YZX"
+        @x = s1 * c2 * c3 + c1 * s2 * s3
+        @y = c1 * s2 * c3 + s1 * c2 * s3
+        @z = c1 * c2 * s3 - s1 * s2 * c3
+        @w = c1 * c2 * c3 - s1 * s2 * s3
+      when "XZY"
+        @x = s1 * c2 * c3 - c1 * s2 * s3
+        @y = c1 * s2 * c3 - s1 * c2 * s3
+        @z = c1 * c2 * s3 + s1 * s2 * c3
+        @w = c1 * c2 * c3 + s1 * s2 * s3
+      end
+
+      self
+    end
+
+    def set_from_rotation_matrix(m)
+      e = m.elements
+
+      m11 = e[0]
+      m12 = e[4]
+      m13 = e[8]
+      m21 = e[1]
+      m22 = e[5]
+      m23 = e[9]
+      m31 = e[2]
+      m32 = e[6]
+      m33 = e[10]
+
+      trace = m11 + m22 + m33
+
+      if trace > 0
+        s = 0.5 / Math.sqrt(trace + 1.0)
+        @w = 0.25 / s
+        @x = (m32 - m23) * s
+        @y = (m13 - m31) * s
+        @z = (m21 - m12) * s
+      elsif m11 > m22 && m11 > m33
+        s = 2.0 * Math.sqrt(1.0 + m11 - m22 - m33)
+        @w = (m32 - m23) / s
+        @x = 0.25 * s
+        @y = (m12 + m21) / s
+        @z = (m13 + m31) / s
+      elsif m22 > m33
+        s = 2.0 * Math.sqrt(1.0 + m22 - m11 - m33)
+        @w = (m13 - m31) / s
+        @x = (m12 + m21) / s
+        @y = 0.25 * s
+        @z = (m23 + m32) / s
+      else
+        s = 2.0 * Math.sqrt(1.0 + m33 - m11 - m22)
+        @w = (m21 - m12) / s
+        @x = (m13 + m31) / s
+        @y = (m23 + m32) / s
+        @z = 0.25 * s
+      end
+
+      self
+    end
+
+    def set_from_unit_vectors(from, to)
+      r = from.dot(to) + 1
+
+      if r < Float::EPSILON
+        r = 0
+        if from.x.abs > from.z.abs
+          @x = -from.y
+          @y = from.x
+          @z = 0
+          @w = r
+        else
+          @x = 0
+          @y = -from.z
+          @z = from.y
+          @w = r
+        end
+      else
+        @x = from.y * to.z - from.z * to.y
+        @y = from.z * to.x - from.x * to.z
+        @z = from.x * to.y - from.y * to.x
+        @w = r
+      end
+
+      normalize!
+    end
+
+    def angle_to(q)
+      2 * Math.acos([dot(q).abs, 1].min)
+    end
+
+    def rotate_towards(q, step)
+      angle = angle_to(q)
+      return self if angle.zero?
+
+      t = [1, step / angle].min
+      slerp(q, t)
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Quaternion)
+
+      (@x - other.x).abs < Float::EPSILON &&
+        (@y - other.y).abs < Float::EPSILON &&
+        (@z - other.z).abs < Float::EPSILON &&
+        (@w - other.w).abs < Float::EPSILON
+    end
+
+    def to_a
+      [@x, @y, @z, @w]
+    end
+
+    def to_h
+      { x: @x, y: @y, z: @z, w: @w }
+    end
+
+    def inspect
+      "#<Sunrb::Quaternion x=#{@x} y=#{@y} z=#{@z} w=#{@w}>"
+    end
+
+    class << self
+      def identity
+        new(0, 0, 0, 1)
+      end
+    end
+  end
+end

data/lib/3rb/math/vector2.rb

@@ -0,0 +1,216 @@
+# frozen_string_literal: true
+
+module Sunrb
+  class Vector2
+    attr_accessor :x, :y
+
+    def initialize(x = 0, y = 0)
+      @x = x.to_f
+      @y = y.to_f
+    end
+
+    def set(x, y)
+      @x = x.to_f
+      @y = y.to_f
+      self
+    end
+
+    def set_scalar(scalar)
+      @x = @y = scalar.to_f
+      self
+    end
+
+    def copy(v)
+      @x = v.x
+      @y = v.y
+      self
+    end
+
+    def clone
+      Vector2.new(@x, @y)
+    end
+
+    def +(other)
+      Vector2.new(@x + other.x, @y + other.y)
+    end
+
+    def -(other)
+      Vector2.new(@x - other.x, @y - other.y)
+    end
+
+    def *(scalar)
+      Vector2.new(@x * scalar, @y * scalar)
+    end
+
+    def /(scalar)
+      Vector2.new(@x / scalar, @y / scalar)
+    end
+
+    def -@
+      Vector2.new(-@x, -@y)
+    end
+
+    def add!(other)
+      @x += other.x
+      @y += other.y
+      self
+    end
+
+    def sub!(other)
+      @x -= other.x
+      @y -= other.y
+      self
+    end
+
+    def multiply_scalar!(scalar)
+      @x *= scalar
+      @y *= scalar
+      self
+    end
+
+    def divide_scalar!(scalar)
+      @x /= scalar
+      @y /= scalar
+      self
+    end
+
+    def dot(other)
+      @x * other.x + @y * other.y
+    end
+
+    def cross(other)
+      @x * other.y - @y * other.x
+    end
+
+    def length_sq
+      @x * @x + @y * @y
+    end
+
+    def length
+      Math.sqrt(length_sq)
+    end
+
+    def normalize
+      clone.normalize!
+    end
+
+    def normalize!
+      len = length
+      return self if len.zero?
+
+      divide_scalar!(len)
+    end
+
+    def distance_to(other)
+      (self - other).length
+    end
+
+    def distance_to_sq(other)
+      (self - other).length_sq
+    end
+
+    def lerp(other, alpha)
+      Vector2.new(
+        @x + (other.x - @x) * alpha,
+        @y + (other.y - @y) * alpha
+      )
+    end
+
+    def lerp!(other, alpha)
+      @x += (other.x - @x) * alpha
+      @y += (other.y - @y) * alpha
+      self
+    end
+
+    def negate!
+      @x = -@x
+      @y = -@y
+      self
+    end
+
+    def min(other)
+      Vector2.new(
+        [@x, other.x].min,
+        [@y, other.y].min
+      )
+    end
+
+    def max(other)
+      Vector2.new(
+        [@x, other.x].max,
+        [@y, other.y].max
+      )
+    end
+
+    def clamp(min_vec, max_vec)
+      Vector2.new(
+        [[@x, min_vec.x].max, max_vec.x].min,
+        [[@y, min_vec.y].max, max_vec.y].min
+      )
+    end
+
+    def floor
+      Vector2.new(@x.floor, @y.floor)
+    end
+
+    def ceil
+      Vector2.new(@x.ceil, @y.ceil)
+    end
+
+    def round
+      Vector2.new(@x.round, @y.round)
+    end
+
+    def angle
+      Math.atan2(-@y, -@x) + Math::PI
+    end
+
+    def rotate_around(center, angle)
+      cos = Math.cos(angle)
+      sin = Math.sin(angle)
+      x = @x - center.x
+      y = @y - center.y
+
+      Vector2.new(
+        x * cos - y * sin + center.x,
+        x * sin + y * cos + center.y
+      )
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Vector2)
+
+      (@x - other.x).abs < Float::EPSILON &&
+        (@y - other.y).abs < Float::EPSILON
+    end
+
+    def equals?(other, tolerance = Float::EPSILON)
+      return false unless other.is_a?(Vector2)
+
+      (@x - other.x).abs <= tolerance &&
+        (@y - other.y).abs <= tolerance
+    end
+
+    def to_a
+      [@x, @y]
+    end
+
+    def to_h
+      { x: @x, y: @y }
+    end
+
+    def inspect
+      "#<Sunrb::Vector2 x=#{@x} y=#{@y}>"
+    end
+
+    class << self
+      def zero
+        new(0, 0)
+      end
+
+      def one
+        new(1, 1)
+      end
+    end
+  end
+end