3rb 0.1.0

data/lib/3rb/math/matrix4.rb

@@ -0,0 +1,501 @@
+# frozen_string_literal: true
+
+module Sunrb
+  class Matrix4
+    attr_reader :elements
+
+    def initialize
+      @elements = [
+        1.0, 0.0, 0.0, 0.0,
+        0.0, 1.0, 0.0, 0.0,
+        0.0, 0.0, 1.0, 0.0,
+        0.0, 0.0, 0.0, 1.0
+      ]
+    end
+
+    def set(n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44)
+      @elements[0] = n11.to_f
+      @elements[4] = n12.to_f
+      @elements[8] = n13.to_f
+      @elements[12] = n14.to_f
+      @elements[1] = n21.to_f
+      @elements[5] = n22.to_f
+      @elements[9] = n23.to_f
+      @elements[13] = n24.to_f
+      @elements[2] = n31.to_f
+      @elements[6] = n32.to_f
+      @elements[10] = n33.to_f
+      @elements[14] = n34.to_f
+      @elements[3] = n41.to_f
+      @elements[7] = n42.to_f
+      @elements[11] = n43.to_f
+      @elements[15] = n44.to_f
+      self
+    end
+
+    def identity
+      set(
+        1, 0, 0, 0,
+        0, 1, 0, 0,
+        0, 0, 1, 0,
+        0, 0, 0, 1
+      )
+    end
+
+    def copy(m)
+      @elements = m.elements.dup
+      self
+    end
+
+    def clone
+      Matrix4.new.copy(self)
+    end
+
+    def *(other)
+      clone.multiply!(other)
+    end
+
+    def multiply!(other)
+      ae = @elements
+      be = other.elements
+
+      a11 = ae[0]
+      a12 = ae[4]
+      a13 = ae[8]
+      a14 = ae[12]
+      a21 = ae[1]
+      a22 = ae[5]
+      a23 = ae[9]
+      a24 = ae[13]
+      a31 = ae[2]
+      a32 = ae[6]
+      a33 = ae[10]
+      a34 = ae[14]
+      a41 = ae[3]
+      a42 = ae[7]
+      a43 = ae[11]
+      a44 = ae[15]
+
+      b11 = be[0]
+      b12 = be[4]
+      b13 = be[8]
+      b14 = be[12]
+      b21 = be[1]
+      b22 = be[5]
+      b23 = be[9]
+      b24 = be[13]
+      b31 = be[2]
+      b32 = be[6]
+      b33 = be[10]
+      b34 = be[14]
+      b41 = be[3]
+      b42 = be[7]
+      b43 = be[11]
+      b44 = be[15]
+
+      @elements[0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41
+      @elements[4] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42
+      @elements[8] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43
+      @elements[12] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44
+
+      @elements[1] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41
+      @elements[5] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42
+      @elements[9] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43
+      @elements[13] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44
+
+      @elements[2] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41
+      @elements[6] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42
+      @elements[10] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43
+      @elements[14] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44
+
+      @elements[3] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41
+      @elements[7] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42
+      @elements[11] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43
+      @elements[15] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44
+
+      self
+    end
+
+    def premultiply!(other)
+      tmp = other * self
+      copy(tmp)
+    end
+
+    def determinant
+      e = @elements
+
+      n11 = e[0]
+      n12 = e[4]
+      n13 = e[8]
+      n14 = e[12]
+      n21 = e[1]
+      n22 = e[5]
+      n23 = e[9]
+      n24 = e[13]
+      n31 = e[2]
+      n32 = e[6]
+      n33 = e[10]
+      n34 = e[14]
+      n41 = e[3]
+      n42 = e[7]
+      n43 = e[11]
+      n44 = e[15]
+
+      n41 * (+n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34) +
+        n42 * (+n11 * n23 * n34 - n11 * n24 * n33 + n14 * n21 * n33 - n13 * n21 * n34 + n13 * n24 * n31 - n14 * n23 * n31) +
+        n43 * (+n11 * n24 * n32 - n11 * n22 * n34 - n14 * n21 * n32 + n12 * n21 * n34 + n14 * n22 * n31 - n12 * n24 * n31) +
+        n44 * (-n13 * n22 * n31 - n11 * n23 * n32 + n11 * n22 * n33 + n13 * n21 * n32 - n12 * n21 * n33 + n12 * n23 * n31)
+    end
+
+    def transpose
+      clone.transpose!
+    end
+
+    def transpose!
+      e = @elements
+      tmp = e[1]
+      e[1] = e[4]
+      e[4] = tmp
+      tmp = e[2]
+      e[2] = e[8]
+      e[8] = tmp
+      tmp = e[6]
+      e[6] = e[9]
+      e[9] = tmp
+      tmp = e[3]
+      e[3] = e[12]
+      e[12] = tmp
+      tmp = e[7]
+      e[7] = e[13]
+      e[13] = tmp
+      tmp = e[11]
+      e[11] = e[14]
+      e[14] = tmp
+      self
+    end
+
+    def invert
+      clone.invert!
+    end
+
+    def invert!
+      e = @elements
+
+      n11 = e[0]
+      n21 = e[1]
+      n31 = e[2]
+      n41 = e[3]
+      n12 = e[4]
+      n22 = e[5]
+      n32 = e[6]
+      n42 = e[7]
+      n13 = e[8]
+      n23 = e[9]
+      n33 = e[10]
+      n43 = e[11]
+      n14 = e[12]
+      n24 = e[13]
+      n34 = e[14]
+      n44 = e[15]
+
+      t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44
+      t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44
+      t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44
+      t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34
+
+      det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14
+
+      return identity if det.zero?
+
+      det_inv = 1.0 / det
+
+      e[0] = t11 * det_inv
+      e[1] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * det_inv
+      e[2] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * det_inv
+      e[3] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * det_inv
+
+      e[4] = t12 * det_inv
+      e[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * det_inv
+      e[6] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * det_inv
+      e[7] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * det_inv
+
+      e[8] = t13 * det_inv
+      e[9] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * det_inv
+      e[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * det_inv
+      e[11] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * det_inv
+
+      e[12] = t14 * det_inv
+      e[13] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * det_inv
+      e[14] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * det_inv
+      e[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * det_inv
+
+      self
+    end
+
+    def compose(position, quaternion, scale)
+      x = quaternion.x
+      y = quaternion.y
+      z = quaternion.z
+      w = quaternion.w
+      x2 = x + x
+      y2 = y + y
+      z2 = z + z
+      xx = x * x2
+      xy = x * y2
+      xz = x * z2
+      yy = y * y2
+      yz = y * z2
+      zz = z * z2
+      wx = w * x2
+      wy = w * y2
+      wz = w * z2
+
+      sx = scale.x
+      sy = scale.y
+      sz = scale.z
+
+      @elements[0] = (1 - (yy + zz)) * sx
+      @elements[1] = (xy + wz) * sx
+      @elements[2] = (xz - wy) * sx
+      @elements[3] = 0
+
+      @elements[4] = (xy - wz) * sy
+      @elements[5] = (1 - (xx + zz)) * sy
+      @elements[6] = (yz + wx) * sy
+      @elements[7] = 0
+
+      @elements[8] = (xz + wy) * sz
+      @elements[9] = (yz - wx) * sz
+      @elements[10] = (1 - (xx + yy)) * sz
+      @elements[11] = 0
+
+      @elements[12] = position.x
+      @elements[13] = position.y
+      @elements[14] = position.z
+      @elements[15] = 1
+
+      self
+    end
+
+    def decompose
+      sx = Vector3.new(@elements[0], @elements[1], @elements[2]).length
+      sy = Vector3.new(@elements[4], @elements[5], @elements[6]).length
+      sz = Vector3.new(@elements[8], @elements[9], @elements[10]).length
+
+      sx = -sx if determinant < 0
+
+      position = Vector3.new(@elements[12], @elements[13], @elements[14])
+
+      m = clone
+      inv_sx = 1.0 / sx
+      inv_sy = 1.0 / sy
+      inv_sz = 1.0 / sz
+
+      m.elements[0] *= inv_sx
+      m.elements[1] *= inv_sx
+      m.elements[2] *= inv_sx
+
+      m.elements[4] *= inv_sy
+      m.elements[5] *= inv_sy
+      m.elements[6] *= inv_sy
+
+      m.elements[8] *= inv_sz
+      m.elements[9] *= inv_sz
+      m.elements[10] *= inv_sz
+
+      quaternion = Quaternion.new.set_from_rotation_matrix(m)
+      scale = Vector3.new(sx, sy, sz)
+
+      [position, quaternion, scale]
+    end
+
+    def make_perspective(fov, aspect, near, far)
+      top = near * Math.tan(fov * 0.5 * Math::PI / 180)
+      height = 2 * top
+      width = aspect * height
+      left = -0.5 * width
+
+      make_perspective_bounds(left, left + width, top, top - height, near, far)
+    end
+
+    def make_perspective_bounds(left, right, top, bottom, near, far)
+      x = 2 * near / (right - left)
+      y = 2 * near / (top - bottom)
+
+      a = (right + left) / (right - left)
+      b = (top + bottom) / (top - bottom)
+      c = -(far + near) / (far - near)
+      d = -2 * far * near / (far - near)
+
+      @elements[0] = x
+      @elements[4] = 0
+      @elements[8] = a
+      @elements[12] = 0
+      @elements[1] = 0
+      @elements[5] = y
+      @elements[9] = b
+      @elements[13] = 0
+      @elements[2] = 0
+      @elements[6] = 0
+      @elements[10] = c
+      @elements[14] = d
+      @elements[3] = 0
+      @elements[7] = 0
+      @elements[11] = -1
+      @elements[15] = 0
+
+      self
+    end
+
+    def make_orthographic(left, right, top, bottom, near, far)
+      w = 1.0 / (right - left)
+      h = 1.0 / (top - bottom)
+      p = 1.0 / (far - near)
+
+      x = (right + left) * w
+      y = (top + bottom) * h
+      z = (far + near) * p
+
+      @elements[0] = 2 * w
+      @elements[4] = 0
+      @elements[8] = 0
+      @elements[12] = -x
+      @elements[1] = 0
+      @elements[5] = 2 * h
+      @elements[9] = 0
+      @elements[13] = -y
+      @elements[2] = 0
+      @elements[6] = 0
+      @elements[10] = -2 * p
+      @elements[14] = -z
+      @elements[3] = 0
+      @elements[7] = 0
+      @elements[11] = 0
+      @elements[15] = 1
+
+      self
+    end
+
+    def make_look_at(eye, target, up)
+      z = (eye - target).normalize
+      z = Vector3.new(0, 0, 1) if z.length_sq.zero?
+
+      x = up.cross(z).normalize
+      x = z.x.abs == 1 ? Vector3.new(0, 0, -z.x) : Vector3.new(1, 0, 0).cross(z).normalize if x.length_sq.zero?
+
+      y = z.cross(x)
+
+      @elements[0] = x.x
+      @elements[4] = y.x
+      @elements[8] = z.x
+      @elements[1] = x.y
+      @elements[5] = y.y
+      @elements[9] = z.y
+      @elements[2] = x.z
+      @elements[6] = y.z
+      @elements[10] = z.z
+
+      self
+    end
+
+    def make_rotation_from_quaternion(q)
+      compose(Vector3.zero, q, Vector3.one)
+    end
+
+    def make_translation(x, y, z)
+      set(
+        1, 0, 0, x,
+        0, 1, 0, y,
+        0, 0, 1, z,
+        0, 0, 0, 1
+      )
+    end
+
+    def make_rotation_x(theta)
+      c = Math.cos(theta)
+      s = Math.sin(theta)
+
+      set(
+        1, 0, 0, 0,
+        0, c, -s, 0,
+        0, s, c, 0,
+        0, 0, 0, 1
+      )
+    end
+
+    def make_rotation_y(theta)
+      c = Math.cos(theta)
+      s = Math.sin(theta)
+
+      set(
+        c, 0, s, 0,
+        0, 1, 0, 0,
+        -s, 0, c, 0,
+        0, 0, 0, 1
+      )
+    end
+
+    def make_rotation_z(theta)
+      c = Math.cos(theta)
+      s = Math.sin(theta)
+
+      set(
+        c, -s, 0, 0,
+        s, c, 0, 0,
+        0, 0, 1, 0,
+        0, 0, 0, 1
+      )
+    end
+
+    def make_scale(x, y, z)
+      set(
+        x, 0, 0, 0,
+        0, y, 0, 0,
+        0, 0, z, 0,
+        0, 0, 0, 1
+      )
+    end
+
+    def get_position
+      Vector3.new(@elements[12], @elements[13], @elements[14])
+    end
+
+    def get_scale
+      sx = Vector3.new(@elements[0], @elements[1], @elements[2]).length
+      sy = Vector3.new(@elements[4], @elements[5], @elements[6]).length
+      sz = Vector3.new(@elements[8], @elements[9], @elements[10]).length
+      Vector3.new(sx, sy, sz)
+    end
+
+    def get_max_scale_on_axis
+      scale = get_scale
+      [scale.x, scale.y, scale.z].max
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Matrix4)
+
+      @elements.each_with_index do |e, i|
+        return false if (e - other.elements[i]).abs > Float::EPSILON
+      end
+      true
+    end
+
+    def to_a
+      @elements.dup
+    end
+
+    def to_matrix3
+      e = @elements
+      Matrix3.new.set(
+        e[0], e[4], e[8],
+        e[1], e[5], e[9],
+        e[2], e[6], e[10]
+      )
+    end
+
+    def inspect
+      "#<Sunrb::Matrix4 #{@elements.inspect}>"
+    end
+  end
+end