larb 0.1.0

data/lib/larb/quat.rb

@@ -0,0 +1,238 @@
+# frozen_string_literal: true
+
+module Larb
+  class Quat
+    attr_accessor :x, :y, :z, :w
+
+    def initialize(x = 0.0, y = 0.0, z = 0.0, w = 1.0)
+      @x = x.to_f
+      @y = y.to_f
+      @z = z.to_f
+      @w = w.to_f
+    end
+
+    def self.[](x, y, z, w)
+      new(x, y, z, w)
+    end
+
+    def self.identity
+      new(0, 0, 0, 1)
+    end
+
+    def self.from_axis_angle(axis, radians)
+      half = radians * 0.5
+      s = Math.sin(half)
+      axis = axis.normalize
+      new(axis.x * s, axis.y * s, axis.z * s, Math.cos(half))
+    end
+
+    def self.from_euler(x, y, z)
+      cx = Math.cos(x * 0.5)
+      sx = Math.sin(x * 0.5)
+      cy = Math.cos(y * 0.5)
+      sy = Math.sin(y * 0.5)
+      cz = Math.cos(z * 0.5)
+      sz = Math.sin(z * 0.5)
+
+      new(
+        sx * cy * cz - cx * sy * sz,
+        cx * sy * cz + sx * cy * sz,
+        cx * cy * sz - sx * sy * cz,
+        cx * cy * cz + sx * sy * sz
+      )
+    end
+
+    def self.look_rotation(forward, up = Vec3.up)
+      forward = forward.normalize
+      right = up.cross(forward).normalize
+      up = forward.cross(right)
+
+      m00 = right.x
+      m01 = up.x
+      m02 = forward.x
+      m10 = right.y
+      m11 = up.y
+      m12 = forward.y
+      m20 = right.z
+      m21 = up.z
+      m22 = forward.z
+
+      trace = m00 + m11 + m22
+      if trace > 0
+        s = 0.5 / Math.sqrt(trace + 1.0)
+        new((m21 - m12) * s, (m02 - m20) * s, (m10 - m01) * s, 0.25 / s)
+      elsif m00 > m11 && m00 > m22
+        s = 2.0 * Math.sqrt(1.0 + m00 - m11 - m22)
+        new(0.25 * s, (m01 + m10) / s, (m02 + m20) / s, (m21 - m12) / s)
+      elsif m11 > m22
+        s = 2.0 * Math.sqrt(1.0 + m11 - m00 - m22)
+        new((m01 + m10) / s, 0.25 * s, (m12 + m21) / s, (m02 - m20) / s)
+      else
+        s = 2.0 * Math.sqrt(1.0 + m22 - m00 - m11)
+        new((m02 + m20) / s, (m12 + m21) / s, 0.25 * s, (m10 - m01) / s)
+      end
+    end
+
+    def *(other)
+      case other
+      when Quat
+        Quat.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
+        )
+      when Vec3
+        qv = Vec3.new(@x, @y, @z)
+        uv = qv.cross(other)
+        uuv = qv.cross(uv)
+        other + (uv * @w + uuv) * 2
+      when Numeric
+        Quat.new(@x * other, @y * other, @z * other, @w * other)
+      end
+    end
+
+    def +(other)
+      Quat.new(@x + other.x, @y + other.y, @z + other.z, @w + other.w)
+    end
+
+    def -(other)
+      Quat.new(@x - other.x, @y - other.y, @z - other.z, @w - other.w)
+    end
+
+    def -@
+      Quat.new(-@x, -@y, -@z, -@w)
+    end
+
+    def dot(other)
+      @x * other.x + @y * other.y + @z * other.z + @w * other.w
+    end
+
+    def length
+      Math.sqrt(length_squared)
+    end
+
+    def length_squared
+      @x * @x + @y * @y + @z * @z + @w * @w
+    end
+
+    def normalize
+      l = length
+      Quat.new(@x / l, @y / l, @z / l, @w / l)
+    end
+
+    def normalize!
+      l = length
+      @x /= l
+      @y /= l
+      @z /= l
+      @w /= l
+      self
+    end
+
+    def conjugate
+      Quat.new(-@x, -@y, -@z, @w)
+    end
+
+    def inverse
+      len_sq = length_squared
+      Quat.new(-@x / len_sq, -@y / len_sq, -@z / len_sq, @w / len_sq)
+    end
+
+    def lerp(other, t)
+      Quat.new(
+        @x + (other.x - @x) * t,
+        @y + (other.y - @y) * t,
+        @z + (other.z - @z) * t,
+        @w + (other.w - @w) * t
+      ).normalize
+    end
+
+    def slerp(other, t)
+      dot_val = dot(other)
+
+      other = -other if dot_val < 0
+      dot_val = dot_val.abs
+
+      if dot_val > 0.9995
+        return lerp(other, t)
+      end
+
+      theta_0 = Math.acos(dot_val.clamp(-1.0, 1.0))
+      theta = theta_0 * t
+      sin_theta = Math.sin(theta)
+      sin_theta_0 = Math.sin(theta_0)
+
+      s0 = Math.cos(theta) - dot_val * sin_theta / sin_theta_0
+      s1 = sin_theta / sin_theta_0
+
+      Quat.new(
+        @x * s0 + other.x * s1,
+        @y * s0 + other.y * s1,
+        @z * s0 + other.z * s1,
+        @w * s0 + other.w * s1
+      )
+    end
+
+    def to_axis_angle
+      angle = 2 * Math.acos(@w.clamp(-1.0, 1.0))
+      s = Math.sqrt(1 - @w * @w)
+      if s < 0.001
+        axis = Vec3.new(1, 0, 0)
+      else
+        axis = Vec3.new(@x / s, @y / s, @z / s)
+      end
+      [axis, angle]
+    end
+
+    def to_euler
+      sinr_cosp = 2 * (@w * @x + @y * @z)
+      cosr_cosp = 1 - 2 * (@x * @x + @y * @y)
+      roll = Math.atan2(sinr_cosp, cosr_cosp)
+
+      sinp = 2 * (@w * @y - @z * @x)
+      pitch = if sinp.abs >= 1
+                Math.copysign(Math::PI / 2, sinp)
+              else
+                Math.asin(sinp)
+              end
+
+      siny_cosp = 2 * (@w * @z + @x * @y)
+      cosy_cosp = 1 - 2 * (@y * @y + @z * @z)
+      yaw = Math.atan2(siny_cosp, cosy_cosp)
+
+      Vec3.new(roll, pitch, yaw)
+    end
+
+    def to_mat4
+      Mat4.from_quaternion(self)
+    end
+
+    def to_a
+      [@x, @y, @z, @w]
+    end
+
+    def [](i)
+      to_a[i]
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Quat)
+
+      @x == other.x && @y == other.y && @z == other.z && @w == other.w
+    end
+
+    def near?(other, epsilon = 1e-6)
+      (@x - other.x).abs < epsilon &&
+        (@y - other.y).abs < epsilon &&
+        (@z - other.z).abs < epsilon &&
+        (@w - other.w).abs < epsilon
+    end
+
+    def inspect
+      "Quat[#{@x}, #{@y}, #{@z}, #{@w}]"
+    end
+
+    alias to_s inspect
+  end
+end

data/lib/larb/quat2.rb

@@ -0,0 +1,193 @@
+# frozen_string_literal: true
+
+module Larb
+  # Dual Quaternion for rigid body transformations
+  # Represented as 8 values: [real quaternion (4), dual quaternion (4)]
+  # real: rotation, dual: translation encoded
+  class Quat2
+    attr_reader :data
+
+    def initialize(data = nil)
+      @data = data || [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]
+    end
+
+    def self.identity
+      new
+    end
+
+    def self.from_rotation_translation(rotation, translation)
+      rx, ry, rz, rw = rotation.x, rotation.y, rotation.z, rotation.w
+      tx, ty, tz = translation.x, translation.y, translation.z
+
+      new([
+        rx, ry, rz, rw,
+        (tx * rw + ty * rz - tz * ry) * 0.5,
+        (ty * rw + tz * rx - tx * rz) * 0.5,
+        (tz * rw + tx * ry - ty * rx) * 0.5,
+        (-tx * rx - ty * ry - tz * rz) * 0.5
+      ])
+    end
+
+    def self.from_translation(translation)
+      from_rotation_translation(Quat.identity, translation)
+    end
+
+    def self.from_rotation(rotation)
+      new([rotation.x, rotation.y, rotation.z, rotation.w, 0, 0, 0, 0])
+    end
+
+    def self.from_mat4(m)
+      rotation = m.extract_rotation
+      translation = m.extract_translation
+      from_rotation_translation(rotation, translation)
+    end
+
+    def real
+      Quat.new(@data[0], @data[1], @data[2], @data[3])
+    end
+
+    def dual
+      Quat.new(@data[4], @data[5], @data[6], @data[7])
+    end
+
+    def [](i)
+      @data[i]
+    end
+
+    def []=(i, v)
+      @data[i] = v.to_f
+    end
+
+    def *(other)
+      case other
+      when Quat2
+        ax0, ay0, az0, aw0 = @data[0], @data[1], @data[2], @data[3]
+        bx1, by1, bz1, bw1 = @data[4], @data[5], @data[6], @data[7]
+        ax1, ay1, az1, aw1 = other.data[0], other.data[1], other.data[2], other.data[3]
+        bx0, by0, bz0, bw0 = other.data[4], other.data[5], other.data[6], other.data[7]
+
+        Quat2.new([
+          ax0 * aw1 + aw0 * ax1 + ay0 * az1 - az0 * ay1,
+          ay0 * aw1 + aw0 * ay1 + az0 * ax1 - ax0 * az1,
+          az0 * aw1 + aw0 * az1 + ax0 * ay1 - ay0 * ax1,
+          aw0 * aw1 - ax0 * ax1 - ay0 * ay1 - az0 * az1,
+          ax0 * bw0 + aw0 * bx0 + ay0 * bz0 - az0 * by0 +
+            bx1 * aw1 + bw1 * ax1 + by1 * az1 - bz1 * ay1,
+          ay0 * bw0 + aw0 * by0 + az0 * bx0 - ax0 * bz0 +
+            by1 * aw1 + bw1 * ay1 + bz1 * ax1 - bx1 * az1,
+          az0 * bw0 + aw0 * bz0 + ax0 * by0 - ay0 * bx0 +
+            bz1 * aw1 + bw1 * az1 + bx1 * ay1 - by1 * ax1,
+          aw0 * bw0 - ax0 * bx0 - ay0 * by0 - az0 * bz0 +
+            bw1 * aw1 - bx1 * ax1 - by1 * ay1 - bz1 * az1
+        ])
+      when Vec3
+        transform_point(other)
+      when Numeric
+        Quat2.new(@data.map { |v| v * other })
+      end
+    end
+
+    def +(other)
+      Quat2.new(@data.zip(other.data).map { |a, b| a + b })
+    end
+
+    def -(other)
+      Quat2.new(@data.zip(other.data).map { |a, b| a - b })
+    end
+
+    def dot(other)
+      @data[0] * other.data[0] + @data[1] * other.data[1] +
+        @data[2] * other.data[2] + @data[3] * other.data[3]
+    end
+
+    def length
+      Math.sqrt(length_squared)
+    end
+
+    def length_squared
+      @data[0..3].sum { |v| v * v }
+    end
+
+    def normalize
+      len = length
+      return Quat2.new(@data.dup) if len < 1e-10
+
+      inv_len = 1.0 / len
+      Quat2.new(@data.map { |v| v * inv_len })
+    end
+
+    def normalize!
+      len = length
+      return self if len < 1e-10
+
+      inv_len = 1.0 / len
+      @data.map! { |v| v * inv_len }
+      self
+    end
+
+    def conjugate
+      Quat2.new([
+        -@data[0], -@data[1], -@data[2], @data[3],
+        -@data[4], -@data[5], -@data[6], @data[7]
+      ])
+    end
+
+    def inverse
+      len_sq = length_squared
+      return conjugate if len_sq < 1e-10
+
+      inv_len_sq = 1.0 / len_sq
+      conj = conjugate
+      Quat2.new(conj.data.map { |v| v * inv_len_sq })
+    end
+
+    def translation
+      ax, ay, az, aw = @data[0], @data[1], @data[2], @data[3]
+      bx, by, bz, bw = @data[4], @data[5], @data[6], @data[7]
+      # t = 2 * dual * conjugate(real)
+      Vec3.new(
+        2 * (-bw * ax + bx * aw - by * az + bz * ay),
+        2 * (-bw * ay + by * aw - bz * ax + bx * az),
+        2 * (-bw * az + bz * aw - bx * ay + by * ax)
+      )
+    end
+
+    def rotation
+      real.normalize
+    end
+
+    def transform_point(point)
+      rotation * point + translation
+    end
+
+    def lerp(other, t)
+      Quat2.new(@data.zip(other.data).map { |a, b| a + (b - a) * t }).normalize
+    end
+
+    def to_mat4
+      rot = rotation
+      trans = translation
+      Mat4.from_quaternion(rot) * Mat4.translation(trans.x, trans.y, trans.z)
+    end
+
+    def to_a
+      @data.dup
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Quat2)
+
+      @data == other.data
+    end
+
+    def near?(other, epsilon = 1e-6)
+      @data.zip(other.data).all? { |a, b| (a - b).abs < epsilon }
+    end
+
+    def inspect
+      "Quat2[real: #{real.inspect}, dual: #{dual.inspect}]"
+    end
+
+    alias to_s inspect
+  end
+end

data/lib/larb/vec2.rb

@@ -0,0 +1,150 @@
+# frozen_string_literal: true
+
+module Larb
+  class Vec2
+    attr_accessor :x, :y
+
+    def initialize(x = 0.0, y = 0.0)
+      @x = x.to_f
+      @y = y.to_f
+    end
+
+    def self.[](x, y)
+      new(x, y)
+    end
+
+    def self.zero
+      new(0, 0)
+    end
+
+    def self.one
+      new(1, 1)
+    end
+
+    def +(other)
+      Vec2.new(@x + other.x, @y + other.y)
+    end
+
+    def -(other)
+      Vec2.new(@x - other.x, @y - other.y)
+    end
+
+    def *(scalar)
+      Vec2.new(@x * scalar, @y * scalar)
+    end
+
+    def /(scalar)
+      Vec2.new(@x / scalar, @y / scalar)
+    end
+
+    def -@
+      Vec2.new(-@x, -@y)
+    end
+
+    def dot(other)
+      @x * other.x + @y * other.y
+    end
+
+    def length
+      Math.sqrt(@x * @x + @y * @y)
+    end
+
+    def length_squared
+      @x * @x + @y * @y
+    end
+
+    def normalize
+      self / length
+    end
+
+    def normalize!
+      l = length
+      @x /= l
+      @y /= l
+      self
+    end
+
+    def lerp(other, t)
+      self + (other - self) * t
+    end
+
+    def to_a
+      [@x, @y]
+    end
+
+    def [](i)
+      to_a[i]
+    end
+
+    def []=(i, v)
+      if i == 0
+        @x = v
+      else
+        @y = v
+      end
+    end
+
+    def ==(other)
+      return false unless other.is_a?(Vec2)
+
+      @x == other.x && @y == other.y
+    end
+
+    def near?(other, epsilon = 1e-6)
+      (@x - other.x).abs < epsilon && (@y - other.y).abs < epsilon
+    end
+
+    def angle
+      Math.atan2(@y, @x)
+    end
+
+    def angle_to(other)
+      Math.atan2(other.y - @y, other.x - @x)
+    end
+
+    def rotate(radians)
+      c = Math.cos(radians)
+      s = Math.sin(radians)
+      Vec2.new(@x * c - @y * s, @x * s + @y * c)
+    end
+
+    def distance(other)
+      Math.sqrt(distance_squared(other))
+    end
+
+    def distance_squared(other)
+      dx = @x - other.x
+      dy = @y - other.y
+      dx * dx + dy * dy
+    end
+
+    def cross(other)
+      @x * other.y - @y * other.x
+    end
+
+    def perpendicular
+      Vec2.new(-@y, @x)
+    end
+
+    def reflect(normal)
+      self - normal * (2 * dot(normal))
+    end
+
+    def clamp_length(max_length)
+      len_sq = length_squared
+      return self if len_sq <= max_length * max_length
+
+      self * (max_length / Math.sqrt(len_sq))
+    end
+
+    def to_vec3(z = 0.0)
+      Vec3.new(@x, @y, z)
+    end
+
+    def inspect
+      "Vec2[#{@x}, #{@y}]"
+    end
+
+    alias to_s inspect
+  end
+end