reze-engine 0.3.4 → 0.3.6

package/src/ik-solver.ts

@@ -5,7 +5,7 @@
  */
 
 import { Mat4, Quat, Vec3 } from "./math"
-import { Bone, IKLink, IKSolver, IKChainInfo, EulerRotationOrder, SolveAxis } from "./model"
+import { Bone, IKLink, IKSolver, IKChainInfo } from "./model"
 
 const enum InternalEulerRotationOrder {
   YXZ = 0,
@@ -89,13 +89,9 @@ export class IKSolverSystem {
     localRotations: Float32Array,
     localTranslations: Float32Array,
     worldMatrices: Float32Array,
-    ikChainInfo: IKChainInfo[],
-    usePhysics: boolean = false
+    ikChainInfo: IKChainInfo[]
   ): void {
     for (const solver of ikSolvers) {
-      if (usePhysics && solver.canSkipWhenPhysicsEnabled) {
-        continue
-      }
       this.solveIK(solver, bones, localRotations, localTranslations, worldMatrices, ikChainInfo)
     }
   }
@@ -121,11 +117,7 @@ export class IKSolverSystem {
       }
     }
 
-    // Get IK bone and target positions
-    const ikPosition = this.getWorldTranslation(ikBoneIndex, worldMatrices)
-    const targetPosition = this.getWorldTranslation(targetBoneIndex, worldMatrices)
-
-    if (ikPosition.subtract(targetPosition).length() < this.EPSILON) return
+    if (this.getDistance(ikBoneIndex, targetBoneIndex, worldMatrices) < this.EPSILON) return
 
     // Build IK chains
     const chains: IKChain[] = []
@@ -135,15 +127,11 @@ export class IKSolverSystem {
 
     // Update chain bones and target bone world matrices (initial state, no IK yet)
     for (let i = chains.length - 1; i >= 0; i--) {
-      this.updateWorldMatrix(chains[i].boneIndex, bones, localRotations, localTranslations, worldMatrices)
+      this.updateWorldMatrix(chains[i].boneIndex, bones, localRotations, localTranslations, worldMatrices, undefined)
     }
-    this.updateWorldMatrix(targetBoneIndex, bones, localRotations, localTranslations, worldMatrices)
-
-    // Re-read positions after initial update
-    const updatedIkPosition = this.getWorldTranslation(ikBoneIndex, worldMatrices)
-    const updatedTargetPosition = this.getWorldTranslation(targetBoneIndex, worldMatrices)
+    this.updateWorldMatrix(targetBoneIndex, bones, localRotations, localTranslations, worldMatrices, undefined)
 
-    if (updatedIkPosition.subtract(updatedTargetPosition).length() < this.EPSILON) return
+    if (this.getDistance(ikBoneIndex, targetBoneIndex, worldMatrices) < this.EPSILON) return
 
     // Solve iteratively
     const iteration = Math.min(solver.iterationCount, 256)
@@ -169,29 +157,17 @@ export class IKSolverSystem {
         }
       }
 
-      // Re-read positions after this iteration
-      const currentIkPosition = this.getWorldTranslation(ikBoneIndex, worldMatrices)
-      const currentTargetPosition = this.getWorldTranslation(targetBoneIndex, worldMatrices)
-      const distance = currentIkPosition.subtract(currentTargetPosition).length()
-      if (distance < this.EPSILON) break
+      if (this.getDistance(ikBoneIndex, targetBoneIndex, worldMatrices) < this.EPSILON) break
     }
 
     // Apply IK rotations to local rotations
     for (const link of solver.links) {
       const chainInfo = ikChainInfo[link.boneIndex]
-      if (chainInfo && chainInfo.ikRotation) {
+      if (chainInfo?.ikRotation) {
         const qi = link.boneIndex * 4
-        const localRot = new Quat(
-          localRotations[qi],
-          localRotations[qi + 1],
-          localRotations[qi + 2],
-          localRotations[qi + 3]
-        )
+        const localRot = this.getQuatFromArray(localRotations, qi)
         const finalRot = chainInfo.ikRotation.multiply(localRot).normalize()
-        localRotations[qi] = finalRot.x
-        localRotations[qi + 1] = finalRot.y
-        localRotations[qi + 2] = finalRot.z
-        localRotations[qi + 3] = finalRot.w
+        this.setQuatToArray(localRotations, qi, finalRot)
       }
     }
   }
@@ -231,25 +207,20 @@ export class IKSolverSystem {
           finalRotationAxis = this.transformNormal(chainRotationAxis, invParentRot).normalize()
           break
         }
-        case InternalSolveAxis.X: {
-          const m = parentWorldRotMatrix.values
-          const axisX = new Vec3(m[0], m[1], m[2])
-          const dot = chainRotationAxis.dot(axisX)
-          finalRotationAxis = new Vec3(dot >= 0 ? 1 : -1, 0, 0)
-          break
-        }
-        case InternalSolveAxis.Y: {
-          const m = parentWorldRotMatrix.values
-          const axisY = new Vec3(m[4], m[5], m[6])
-          const dot = chainRotationAxis.dot(axisY)
-          finalRotationAxis = new Vec3(0, dot >= 0 ? 1 : -1, 0)
-          break
-        }
+        case InternalSolveAxis.X:
+        case InternalSolveAxis.Y:
         case InternalSolveAxis.Z: {
           const m = parentWorldRotMatrix.values
-          const axisZ = new Vec3(m[8], m[9], m[10])
-          const dot = chainRotationAxis.dot(axisZ)
-          finalRotationAxis = new Vec3(0, 0, dot >= 0 ? 1 : -1)
+          const axisOffset = (chain.solveAxis - InternalSolveAxis.X) * 4
+          const axis = new Vec3(m[axisOffset], m[axisOffset + 1], m[axisOffset + 2])
+          const dot = chainRotationAxis.dot(axis)
+          const sign = dot >= 0 ? 1 : -1
+          finalRotationAxis =
+            chain.solveAxis === InternalSolveAxis.X
+              ? new Vec3(sign, 0, 0)
+              : chain.solveAxis === InternalSolveAxis.Y
+              ? new Vec3(0, sign, 0)
+              : new Vec3(0, 0, sign)
           break
         }
         default:
@@ -271,93 +242,25 @@ export class IKSolverSystem {
       chainInfo.ikRotation = ikRotation.multiply(chainInfo.ikRotation)
 
       // Apply angle constraints if present
-      if (chain.minimumAngle !== null && chain.maximumAngle !== null) {
+      if (chain.minimumAngle && chain.maximumAngle) {
         const qi = chainBoneIndex * 4
-        const localRot = new Quat(
-          localRotations[qi],
-          localRotations[qi + 1],
-          localRotations[qi + 2],
-          localRotations[qi + 3]
-        )
+        const localRot = this.getQuatFromArray(localRotations, qi)
         chainInfo.localRotation = localRot.clone()
 
         const combinedRot = chainInfo.ikRotation.multiply(localRot)
-        const rotMatrix = Mat4.fromQuat(combinedRot.x, combinedRot.y, combinedRot.z, combinedRot.w)
-        const m = rotMatrix.values
-
-        let rX: number, rY: number, rZ: number
-
-        switch (chain.rotationOrder) {
-          case InternalEulerRotationOrder.YXZ: {
-            rX = Math.asin(-m[9]) // m32
-            if (Math.abs(rX) > this.THRESHOLD) {
-              rX = rX < 0 ? -this.THRESHOLD : this.THRESHOLD
-            }
-            let cosX = Math.cos(rX)
-            if (cosX !== 0) cosX = 1 / cosX
-            rY = Math.atan2(m[8] * cosX, m[10] * cosX) // m31, m33
-            rZ = Math.atan2(m[1] * cosX, m[5] * cosX) // m12, m22
-
-            rX = this.limitAngle(rX, chain.minimumAngle.x, chain.maximumAngle.x, useAxis)
-            rY = this.limitAngle(rY, chain.minimumAngle.y, chain.maximumAngle.y, useAxis)
-            rZ = this.limitAngle(rZ, chain.minimumAngle.z, chain.maximumAngle.z, useAxis)
-
-            chainInfo.ikRotation = Quat.fromAxisAngle(new Vec3(0, 1, 0), rY)
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(1, 0, 0), rX))
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(0, 0, 1), rZ))
-            break
-          }
-          case InternalEulerRotationOrder.ZYX: {
-            rY = Math.asin(-m[2]) // m13
-            if (Math.abs(rY) > this.THRESHOLD) {
-              rY = rY < 0 ? -this.THRESHOLD : this.THRESHOLD
-            }
-            let cosY = Math.cos(rY)
-            if (cosY !== 0) cosY = 1 / cosY
-            rX = Math.atan2(m[6] * cosY, m[10] * cosY) // m23, m33
-            rZ = Math.atan2(m[1] * cosY, m[0] * cosY) // m12, m11
-
-            rX = this.limitAngle(rX, chain.minimumAngle.x, chain.maximumAngle.x, useAxis)
-            rY = this.limitAngle(rY, chain.minimumAngle.y, chain.maximumAngle.y, useAxis)
-            rZ = this.limitAngle(rZ, chain.minimumAngle.z, chain.maximumAngle.z, useAxis)
-
-            chainInfo.ikRotation = Quat.fromAxisAngle(new Vec3(0, 0, 1), rZ)
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(0, 1, 0), rY))
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(1, 0, 0), rX))
-            break
-          }
-          case InternalEulerRotationOrder.XZY: {
-            rZ = Math.asin(-m[4]) // m21
-            if (Math.abs(rZ) > this.THRESHOLD) {
-              rZ = rZ < 0 ? -this.THRESHOLD : this.THRESHOLD
-            }
-            let cosZ = Math.cos(rZ)
-            if (cosZ !== 0) cosZ = 1 / cosZ
-            rX = Math.atan2(m[6] * cosZ, m[5] * cosZ) // m23, m22
-            rY = Math.atan2(m[8] * cosZ, m[0] * cosZ) // m31, m11
-
-            rX = this.limitAngle(rX, chain.minimumAngle.x, chain.maximumAngle.x, useAxis)
-            rY = this.limitAngle(rY, chain.minimumAngle.y, chain.maximumAngle.y, useAxis)
-            rZ = this.limitAngle(rZ, chain.minimumAngle.z, chain.maximumAngle.z, useAxis)
-
-            chainInfo.ikRotation = Quat.fromAxisAngle(new Vec3(1, 0, 0), rX)
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(0, 0, 1), rZ))
-            chainInfo.ikRotation = chainInfo.ikRotation.multiply(Quat.fromAxisAngle(new Vec3(0, 1, 0), rY))
-            break
-          }
-        }
-
-        const invertedLocalRotation = localRot.conjugate().normalize()
-        chainInfo.ikRotation = chainInfo.ikRotation.multiply(invertedLocalRotation)
+        const euler = this.extractEulerAngles(combinedRot, chain.rotationOrder)
+        const limited = this.limitEulerAngles(euler, chain.minimumAngle, chain.maximumAngle, useAxis)
+        chainInfo.ikRotation = this.reconstructQuatFromEuler(limited, chain.rotationOrder)
+        chainInfo.ikRotation = chainInfo.ikRotation.multiply(localRot.conjugate().normalize())
       }
     }
 
     // Update world matrices for affected bones (using IK-modified rotations)
     for (let i = chainIndex; i >= 0; i--) {
       const link = solver.links[i]
-      this.updateWorldMatrixWithIK(link.boneIndex, bones, localRotations, localTranslations, worldMatrices, ikChainInfo)
+      this.updateWorldMatrix(link.boneIndex, bones, localRotations, localTranslations, worldMatrices, ikChainInfo)
     }
-    this.updateWorldMatrix(targetBoneIndex, bones, localRotations, localTranslations, worldMatrices)
+    this.updateWorldMatrix(targetBoneIndex, bones, localRotations, localTranslations, worldMatrices, undefined)
   }
 
   private static limitAngle(angle: number, min: number, max: number, useAxis: boolean): number {
@@ -372,11 +275,92 @@ export class IKSolverSystem {
     }
   }
 
+  private static getDistance(boneIndex1: number, boneIndex2: number, worldMatrices: Float32Array): number {
+    const pos1 = this.getWorldTranslation(boneIndex1, worldMatrices)
+    const pos2 = this.getWorldTranslation(boneIndex2, worldMatrices)
+    return pos1.subtract(pos2).length()
+  }
+
   private static getWorldTranslation(boneIndex: number, worldMatrices: Float32Array): Vec3 {
     const offset = boneIndex * 16
     return new Vec3(worldMatrices[offset + 12], worldMatrices[offset + 13], worldMatrices[offset + 14])
   }
 
+  private static getQuatFromArray(array: Float32Array, offset: number): Quat {
+    return new Quat(array[offset], array[offset + 1], array[offset + 2], array[offset + 3])
+  }
+
+  private static setQuatToArray(array: Float32Array, offset: number, quat: Quat): void {
+    array[offset] = quat.x
+    array[offset + 1] = quat.y
+    array[offset + 2] = quat.z
+    array[offset + 3] = quat.w
+  }
+
+  private static extractEulerAngles(quat: Quat, order: InternalEulerRotationOrder): Vec3 {
+    const rotMatrix = Mat4.fromQuat(quat.x, quat.y, quat.z, quat.w)
+    const m = rotMatrix.values
+
+    switch (order) {
+      case InternalEulerRotationOrder.YXZ: {
+        let rX = Math.asin(-m[9])
+        if (Math.abs(rX) > this.THRESHOLD) rX = rX < 0 ? -this.THRESHOLD : this.THRESHOLD
+        let cosX = Math.cos(rX)
+        if (cosX !== 0) cosX = 1 / cosX
+        const rY = Math.atan2(m[8] * cosX, m[10] * cosX)
+        const rZ = Math.atan2(m[1] * cosX, m[5] * cosX)
+        return new Vec3(rX, rY, rZ)
+      }
+      case InternalEulerRotationOrder.ZYX: {
+        let rY = Math.asin(-m[2])
+        if (Math.abs(rY) > this.THRESHOLD) rY = rY < 0 ? -this.THRESHOLD : this.THRESHOLD
+        let cosY = Math.cos(rY)
+        if (cosY !== 0) cosY = 1 / cosY
+        const rX = Math.atan2(m[6] * cosY, m[10] * cosY)
+        const rZ = Math.atan2(m[1] * cosY, m[0] * cosY)
+        return new Vec3(rX, rY, rZ)
+      }
+      case InternalEulerRotationOrder.XZY: {
+        let rZ = Math.asin(-m[4])
+        if (Math.abs(rZ) > this.THRESHOLD) rZ = rZ < 0 ? -this.THRESHOLD : this.THRESHOLD
+        let cosZ = Math.cos(rZ)
+        if (cosZ !== 0) cosZ = 1 / cosZ
+        const rX = Math.atan2(m[6] * cosZ, m[5] * cosZ)
+        const rY = Math.atan2(m[8] * cosZ, m[0] * cosZ)
+        return new Vec3(rX, rY, rZ)
+      }
+    }
+  }
+
+  private static limitEulerAngles(euler: Vec3, min: Vec3, max: Vec3, useAxis: boolean): Vec3 {
+    return new Vec3(
+      this.limitAngle(euler.x, min.x, max.x, useAxis),
+      this.limitAngle(euler.y, min.y, max.y, useAxis),
+      this.limitAngle(euler.z, min.z, max.z, useAxis)
+    )
+  }
+
+  private static reconstructQuatFromEuler(euler: Vec3, order: InternalEulerRotationOrder): Quat {
+    const axes = [
+      [new Vec3(1, 0, 0), new Vec3(0, 1, 0), new Vec3(0, 0, 1)],
+      [new Vec3(0, 0, 1), new Vec3(0, 1, 0), new Vec3(1, 0, 0)],
+      [new Vec3(0, 1, 0), new Vec3(1, 0, 0), new Vec3(0, 0, 1)],
+    ]
+
+    const [axis1, axis2, axis3] = axes[order]
+    const [angle1, angle2, angle3] =
+      order === InternalEulerRotationOrder.YXZ
+        ? [euler.y, euler.x, euler.z]
+        : order === InternalEulerRotationOrder.ZYX
+        ? [euler.z, euler.y, euler.x]
+        : [euler.x, euler.z, euler.y]
+
+    let result = Quat.fromAxisAngle(axis1, angle1)
+    result = result.multiply(Quat.fromAxisAngle(axis2, angle2))
+    result = result.multiply(Quat.fromAxisAngle(axis3, angle3))
+    return result
+  }
+
   private static getParentWorldRotationMatrix(boneIndex: number, bones: Bone[], worldMatrices: Float32Array): Mat4 {
     const bone = bones[boneIndex]
     if (bone.parentIndex >= 0) {
@@ -401,29 +385,27 @@ export class IKSolverSystem {
     )
   }
 
-  private static updateWorldMatrixWithIK(
+  private static updateWorldMatrix(
     boneIndex: number,
     bones: Bone[],
     localRotations: Float32Array,
     localTranslations: Float32Array,
     worldMatrices: Float32Array,
-    ikChainInfo: IKChainInfo[]
+    ikChainInfo?: IKChainInfo[]
   ): void {
     const bone = bones[boneIndex]
     const qi = boneIndex * 4
     const ti = boneIndex * 3
 
-    // Use IK-modified rotation if available
-    const localRot = new Quat(
-      localRotations[qi],
-      localRotations[qi + 1],
-      localRotations[qi + 2],
-      localRotations[qi + 3]
-    )
-    const chainInfo = ikChainInfo[boneIndex]
+    const localRot = this.getQuatFromArray(localRotations, qi)
+
+    // Apply IK rotation if available
     let finalRot = localRot
-    if (chainInfo && chainInfo.ikRotation) {
-      finalRot = chainInfo.ikRotation.multiply(localRot).normalize()
+    if (ikChainInfo) {
+      const chainInfo = ikChainInfo[boneIndex]
+      if (chainInfo && chainInfo.ikRotation) {
+        finalRot = chainInfo.ikRotation.multiply(localRot).normalize()
+      }
     }
     const rotateM = Mat4.fromQuat(finalRot.x, finalRot.y, finalRot.z, finalRot.w)
 
@@ -446,43 +428,4 @@ export class IKSolverSystem {
       worldMatrices.subarray(worldOffset, worldOffset + 16).set(localM.values)
     }
   }
-
-  private static updateWorldMatrix(
-    boneIndex: number,
-    bones: Bone[],
-    localRotations: Float32Array,
-    localTranslations: Float32Array,
-    worldMatrices: Float32Array
-  ): void {
-    const bone = bones[boneIndex]
-    const qi = boneIndex * 4
-    const ti = boneIndex * 3
-
-    const localRot = new Quat(
-      localRotations[qi],
-      localRotations[qi + 1],
-      localRotations[qi + 2],
-      localRotations[qi + 3]
-    )
-    const rotateM = Mat4.fromQuat(localRot.x, localRot.y, localRot.z, localRot.w)
-
-    const localTx = localTranslations[ti]
-    const localTy = localTranslations[ti + 1]
-    const localTz = localTranslations[ti + 2]
-
-    const localM = Mat4.identity()
-      .translateInPlace(bone.bindTranslation[0], bone.bindTranslation[1], bone.bindTranslation[2])
-      .multiply(rotateM)
-      .translateInPlace(localTx, localTy, localTz)
-
-    const worldOffset = boneIndex * 16
-    if (bone.parentIndex >= 0) {
-      const parentOffset = bone.parentIndex * 16
-      const parentMat = new Mat4(worldMatrices.subarray(parentOffset, parentOffset + 16))
-      const worldMat = parentMat.multiply(localM)
-      worldMatrices.subarray(worldOffset, worldOffset + 16).set(worldMat.values)
-    } else {
-      worldMatrices.subarray(worldOffset, worldOffset + 16).set(localM.values)
-    }
-  }
 }

package/src/math.ts

@@ -26,10 +26,6 @@ export class Vec3 {
     return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z)
   }
 
-  lengthSquared(): number {
-    return this.x * this.x + this.y * this.y + this.z * this.z
-  }
-
   normalize(): Vec3 {
     const len = this.length()
     if (len === 0) return new Vec3(0, 0, 0)
@@ -51,10 +47,6 @@ export class Vec3 {
   scale(scalar: number): Vec3 {
     return new Vec3(this.x * scalar, this.y * scalar, this.z * scalar)
   }
-
-  clone(): Vec3 {
-    return new Vec3(this.x, this.y, this.z)
-  }
 }
 
 export class Quat {
@@ -103,55 +95,6 @@ export class Quat {
     return new Quat(this.x / len, this.y / len, this.z / len, this.w / len)
   }
 
-  // Rotate a vector by this quaternion: result = q * v * q^-1
-  rotateVec(v: Vec3): Vec3 {
-    // Treat v as pure quaternion (x, y, z, 0)
-    const qx = this.x,
-      qy = this.y,
-      qz = this.z,
-      qw = this.w
-    const vx = v.x,
-      vy = v.y,
-      vz = v.z
-
-    // t = 2 * cross(q.xyz, v)
-    const tx = 2 * (qy * vz - qz * vy)
-    const ty = 2 * (qz * vx - qx * vz)
-    const tz = 2 * (qx * vy - qy * vx)
-
-    // result = v + q.w * t + cross(q.xyz, t)
-    return new Vec3(
-      vx + qw * tx + (qy * tz - qz * ty),
-      vy + qw * ty + (qz * tx - qx * tz),
-      vz + qw * tz + (qx * ty - qy * tx)
-    )
-  }
-
-  // Rotate a vector by this quaternion (Babylon.js style naming)
-  rotate(v: Vec3): Vec3 {
-    const qv = new Vec3(this.x, this.y, this.z)
-    const uv = qv.cross(v)
-    const uuv = qv.cross(uv)
-    return v.add(uv.scale(2 * this.w)).add(uuv.scale(2))
-  }
-
-  // Static method: create quaternion that rotates from one direction to another
-  static fromTo(from: Vec3, to: Vec3): Quat {
-    const dot = from.dot(to)
-    if (dot > 0.999999) return new Quat(0, 0, 0, 1) // Already aligned
-    if (dot < -0.999999) {
-      // 180 degrees
-      let axis = from.cross(new Vec3(1, 0, 0))
-      if (axis.length() < 0.001) axis = from.cross(new Vec3(0, 1, 0))
-      return new Quat(axis.x, axis.y, axis.z, 0).normalize()
-    }
-
-    const axis = from.cross(to)
-    const w = Math.sqrt((1 + dot) * 2)
-    const invW = 1 / w
-    return new Quat(axis.x * invW, axis.y * invW, axis.z * invW, w * 0.5).normalize()
-  }
-
   // Static method: create quaternion from rotation axis and angle
   static fromAxisAngle(axis: Vec3, angle: number): Quat {
     const normalizedAxis = axis.normalize()
@@ -217,31 +160,6 @@ export class Quat {
 
     return new Quat(x, y, z, w).normalize()
   }
-
-  // Convert quaternion to Euler angles (ZXY order, inverse of fromEuler)
-  toEuler(): Vec3 {
-    const qx = this.x
-    const qy = this.y
-    const qz = this.z
-    const qw = this.w
-
-    // ZXY order (left-handed)
-    // Roll (X): rotation around X axis
-    const sinr_cosp = 2 * (qw * qx + qy * qz)
-    const cosr_cosp = 1 - 2 * (qx * qx + qy * qy)
-    const rotX = Math.atan2(sinr_cosp, cosr_cosp)
-
-    // Pitch (Y): rotation around Y axis
-    const sinp = 2 * (qw * qy - qz * qx)
-    const rotY = Math.abs(sinp) >= 1 ? (sinp >= 0 ? Math.PI / 2 : -Math.PI / 2) : Math.asin(sinp)
-
-    // Yaw (Z): rotation around Z axis
-    const siny_cosp = 2 * (qw * qz + qx * qy)
-    const cosy_cosp = 1 - 2 * (qy * qy + qz * qz)
-    const rotZ = Math.atan2(siny_cosp, cosy_cosp)
-
-    return new Vec3(rotX, rotY, rotZ)
-  }
 }
 
 export class Mat4 {
@@ -557,3 +475,46 @@ export class Mat4 {
     return new Mat4(out)
   }
 }
+
+/**
+ * Bezier interpolation function
+ * @param x1 First control point X (0-127, normalized to 0-1)
+ * @param x2 Second control point X (0-127, normalized to 0-1)
+ * @param y1 First control point Y (0-127, normalized to 0-1)
+ * @param y2 Second control point Y (0-127, normalized to 0-1)
+ * @param t Interpolation parameter (0-1)
+ * @returns Interpolated value (0-1)
+ */
+export function bezierInterpolate(x1: number, x2: number, y1: number, y2: number, t: number): number {
+  // Clamp t to [0, 1]
+  t = Math.max(0, Math.min(1, t))
+
+  // Binary search for the t value that gives us the desired x
+  // We're solving for t in the Bezier curve: x(t) = 3*(1-t)^2*t*x1 + 3*(1-t)*t^2*x2 + t^3
+  let start = 0
+  let end = 1
+  let mid = 0.5
+
+  // Iterate until we find the t value that gives us the desired x
+  for (let i = 0; i < 15; i++) {
+    // Evaluate Bezier curve at mid point
+    const x = 3 * (1 - mid) * (1 - mid) * mid * x1 + 3 * (1 - mid) * mid * mid * x2 + mid * mid * mid
+
+    if (Math.abs(x - t) < 0.0001) {
+      break
+    }
+
+    if (x < t) {
+      start = mid
+    } else {
+      end = mid
+    }
+
+    mid = (start + end) / 2
+  }
+
+  // Now evaluate the y value at this t
+  const y = 3 * (1 - mid) * (1 - mid) * mid * y1 + 3 * (1 - mid) * mid * mid * y2 + mid * mid * mid
+
+  return y
+}