Merge pull request #10 from 101island/feature/physics

Feature/physics

src/config/marbleConfig.ts

@@ -27,7 +27,7 @@ export const MARBLE_CONFIG = {
     restitution: 0.92, // Marble collision restitution coefficient (0-1)
     wallBounce: 0.85, // Wall bounce coefficient (0-1)
     minSpeed: 50, // Minimum speed threshold. Below this value, speed increases to this value: CurrentSpeed *= scale = minSpeed / speed
-    maxSpeed: 800, // Global maximum speed limit, limited in the same way as above
+    maxSpeed: 3200, // Global maximum speed limit, limited in the same way as above
   },
 
   // Animation configuration

src/utils/marblePhysics.ts

@@ -1,7 +1,16 @@
 // Marble physics system: Handles marble movement, collision detection, and boundary processing
+// Refactored to use XPBD (Extended Position Based Dynamics) for better stability
 
 import type { Marble } from "./mouseInteraction";
 
+// Extend Marble interface to include previous positions for XPBD
+interface PhysicsMarble extends Marble {
+  prevX?: number;
+  prevY?: number;
+  prevVx?: number;
+  prevVy?: number;
+}
+
 export interface PhysicsConfig {
   fieldWidth: number;
   fieldHeight: number;
@@ -15,8 +24,17 @@ export interface PhysicsConfig {
   debugVectorScale?: number; // Scale factor for velocity vectors (default: 0.5)
 }
 
+interface Contact {
+  a: PhysicsMarble;
+  b: PhysicsMarble;
+  nx: number;
+  ny: number;
+  dist: number; // Penetration depth or distance
+}
+
 export class MarblePhysics {
-  private config: PhysicsConfig;
+  public config: PhysicsConfig;
+  private currentContacts: Contact[] = [];
 
   constructor(config: PhysicsConfig) {
     this.config = {
@@ -33,19 +51,28 @@ export class MarblePhysics {
     };
   }
 
-  // Update marble positions
-  public updatePositions(marbles: Marble[], dt: number): void {
+  // 1. Integration Step: Apply external forces and predict new positions
+  // XPBD: x' = x + v * dt
+  public updatePositions(marbles: PhysicsMarble[], dt: number): void {
     const { damping, minSpeed, maxSpeed } = this.config;
 
     for (const m of marbles) {
-      // Apply air resistance
+      // Store Velocity for Restitution (XPBD needs pre-solve velocity)
+      m.prevVx = m.vx;
+      m.prevVy = m.vy;
+
+      // Initialize prev positions if missing
+      if (m.prevX === undefined) m.prevX = m.x;
+      if (m.prevY === undefined) m.prevY = m.y;
+
+      // Apply external forces to velocity (e.g. Damping)
       if (damping !== undefined && damping < 1) {
-        const dampingFactor = damping ** (dt * 60); // Frame rate independent
+        const dampingFactor = damping ** (dt * 60);
         m.vx *= dampingFactor;
         m.vy *= dampingFactor;
       }
 
-      // Calculate current speed
+      // Limit max speed
       const speed = Math.hypot(m.vx, m.vy);
 
       // Maintain minimum speed (prevent complete stop)
@@ -62,17 +89,36 @@ export class MarblePhysics {
         m.vy *= scale;
       }
 
-      // Update position
+      // Store current position as previous
+      m.prevX = m.x;
+      m.prevY = m.y;
+
+      // Predict new position
       m.x += m.vx * dt;
       m.y += m.vy * dt;
     }
   }
 
-  // Handle collisions between marbles using Spatial Grid
-  public handleCollisions(marbles: Marble[]): void {
-    if (this.config.enableCollisions === false) return;
+  // 2. Constraint Solving: Correct positions to satisfy constraints
+  public handleCollisions(marbles: PhysicsMarble[]): void {
+    const { enableCollisions, fieldWidth, fieldHeight } = this.config;
+    this.currentContacts = []; // Clear previous contacts
 
-    const restitution = this.config.restitution ?? 1;
+    // --- Boundary Constraints (Position Correction) ---
+    for (const m of marbles) {
+      if (m.x < m.radius) m.x = m.radius;
+      if (m.x > fieldWidth - m.radius) m.x = fieldWidth - m.radius;
+      if (m.y < m.radius) m.y = m.radius;
+      if (m.y > fieldHeight - m.radius) m.y = fieldHeight - m.radius;
+    }
+
+    // --- Marble-Marble Collisions (Grid-based) ---
+    if (enableCollisions !== false) {
+      this.handleMarbleCollisions(marbles);
+    }
+  }
+
+  private handleMarbleCollisions(marbles: PhysicsMarble[]) {
     const { fieldWidth, fieldHeight } = this.config;
 
     // 1. Determine grid cell size
@@ -91,27 +137,22 @@ export class MarblePhysics {
 
     // 2. Build the grid
     // Map: cellIndex -> Particle[]
-    const grid = new Map<number, Marble[]>();
-
+    const grid = new Map<number, PhysicsMarble[]>();
     const getGridIndex = (x: number, y: number) => {
       const gx = Math.floor(x / cellSize);
       const gy = Math.floor(y / cellSize);
-      // Clamp to valid range to handle out-of-bounds marbles gracefully
       if (gx < 0 || gx >= gridWidth || gy < 0 || gy >= gridHeight) return -1;
       return gx + gy * gridWidth;
     };
 
     for (const m of marbles) {
       const index = getGridIndex(m.x, m.y);
-      if (index === -1) continue; // Skip out of bounds marbles (handled by boundaries)
-
-      if (!grid.has(index)) {
-        grid.set(index, []);
-      }
+      if (index === -1) continue;
+      if (!grid.has(index)) grid.set(index, []);
       grid.get(index)?.push(m);
     }
 
-    // 3. Check collisions (Grid-based)
+    // 3. Solve Collisions (Grid-based)
     // We iterate through each marble, find its cell, and check that cell + neighbors
     for (const i_marble of marbles) {
       const gx = Math.floor(i_marble.x / cellSize);
@@ -127,14 +168,13 @@ export class MarblePhysics {
         for (let ny = gy - 1; ny <= gy + 1; ny++) {
           if (nx < 0 || nx >= gridWidth || ny < 0 || ny >= gridHeight) continue;
 
-          const neighborIndex = nx + ny * gridWidth;
-          const cellMarbles = grid.get(neighborIndex);
-
+          const cellMarbles = grid.get(nx + ny * gridWidth);
           if (!cellMarbles) continue;
 
           for (const j_marble of cellMarbles) {
             // Avoid self-collision
             if (i_marble === j_marble) continue;
+            if (i_marble.id >= j_marble.id) continue; // Check unique pair
 
             // Avoid double checking: only check if index(i) < index(j)
             // But here we rely on the object content.
@@ -145,7 +185,6 @@ export class MarblePhysics {
             // Ideally: We iterate unique pairs.
             // Optimization: Only check half-neighborhood?
             // Or simpler: check all, but only act if i_marble.id < j_marble.id
-            if (i_marble.id >= j_marble.id) continue;
 
             const a = i_marble;
             const b = j_marble;
@@ -158,26 +197,37 @@ export class MarblePhysics {
 
             if (distSq < minDistSq) {
               const dist = Math.sqrt(distSq) || 0.001;
-              const nx = dx / dist;
-              const ny = dy / dist;
-              const relativeVelocity = (a.vx - b.vx) * nx + (a.vy - b.vy) * ny;
+              const nX = dx / dist;
+              const nY = dy / dist;
+              const penetration = minDist - dist;
 
-              if (relativeVelocity > 0) {
-                // Calculate impulse using restitution coefficient
-                const impulse =
-                  ((1 + restitution) * relativeVelocity) / (a.mass + b.mass);
-                a.vx -= impulse * b.mass * nx;
-                a.vy -= impulse * b.mass * ny;
-                b.vx += impulse * a.mass * nx;
-                b.vy += impulse * a.mass * ny;
-              }
+              // XPBD Position Correction
+              const wA = 1 / a.mass;
+              const wB = 1 / b.mass;
+              const wSum = wA + wB;
 
-              // Position correction to prevent overlap
-              const overlap = minDist - dist + 0.5;
-              a.x -= nx * overlap * 0.5;
-              a.y -= ny * overlap * 0.5;
-              b.x += nx * overlap * 0.5;
-              b.y += ny * overlap * 0.5;
+              if (wSum === 0) continue;
+
+              // dx_p = (w / wSum) * penetration * n
+              // Compliance = 0 (hard constraint)
+              const lambda = penetration / wSum;
+
+              const deltaX = nX * lambda;
+              const deltaY = nY * lambda;
+
+              a.x -= deltaX * wA;
+              a.y -= deltaY * wA;
+              b.x += deltaX * wB;
+              b.y += deltaY * wB;
+
+              // Store contact for velocity resolve
+              this.currentContacts.push({
+                a,
+                b,
+                nx: nX,
+                ny: nY,
+                dist: penetration,
+              });
             }
           }
         }
@@ -185,32 +235,73 @@ export class MarblePhysics {
     }
   }
 
-  // Handle boundary collisions
-  public handleBoundaries(marbles: Marble[]): void {
+  // 3. Velocity Update (and Resolve)
+  // XPBD: v = (x - prevX) / dt
+  // Then apply restitution to v
+  public resolveVelocities(marbles: PhysicsMarble[], dt: number): void {
+    // 3a. Update velocities from position change
+    for (const m of marbles) {
+      if (m.prevX !== undefined && m.prevY !== undefined) {
+        m.vx = (m.x - m.prevX) / dt;
+        m.vy = (m.y - m.prevY) / dt;
+      }
+    }
+
+    // 3b. Apply Wall Bounce (Velocity Reflection)
     const { fieldWidth, fieldHeight, wallBounce } = this.config;
-    const bounce = wallBounce ?? 1;
+    const e = wallBounce ?? 0.85;
 
     for (const m of marbles) {
-      // Left boundary
-      if (m.x - m.radius < 0) {
-        m.x = m.radius;
-        if (m.vx < 0) m.vx *= -bounce;
-      }
-      // Right boundary
-      if (m.x + m.radius > fieldWidth) {
-        m.x = fieldWidth - m.radius;
-        if (m.vx > 0) m.vx *= -bounce;
-      }
-      // Top boundary
-      if (m.y - m.radius < 0) {
-        m.y = m.radius;
-        if (m.vy < 0) m.vy *= -bounce;
-      }
-      // Bottom boundary
-      if (m.y + m.radius > fieldHeight) {
-        m.y = fieldHeight - m.radius;
-        if (m.vy > 0) m.vy *= -bounce;
-      }
+      // Left Wall
+      if (m.x <= m.radius + 0.5 && m.vx < 0) m.vx *= -e;
+      // Right Wall
+      if (m.x >= fieldWidth - m.radius - 0.5 && m.vx > 0) m.vx *= -e;
+      // Top Wall
+      if (m.y <= m.radius + 0.5 && m.vy < 0) m.vy *= -e;
+      // Bottom Wall
+      if (m.y >= fieldHeight - m.radius - 0.5 && m.vy > 0) m.vy *= -e;
+    }
+
+    // 3c. Apply Marble Restitution
+    this.applyRestitution();
+  }
+
+  private applyRestitution() {
+    const restitution = this.config.restitution ?? 0.92;
+
+    for (const contact of this.currentContacts) {
+      const { a, b, nx, ny } = contact;
+      // Use stored pre-velocities
+      const vax = a.prevVx ?? 0;
+      const vay = a.prevVy ?? 0;
+      const vbx = b.prevVx ?? 0;
+      const vby = b.prevVy ?? 0;
+
+      const dvx = vbx - vax;
+      const dvy = vby - vay;
+      const vn_pre = dvx * nx + dvy * ny;
+
+      // If separating already, skip
+      if (vn_pre > 0) continue;
+
+      // Target: vn_final = -e * vn_pre
+      const vn_current = (b.vx - a.vx) * nx + (b.vy - a.vy) * ny;
+      const vn_goal = -restitution * vn_pre;
+      const delta_vn = vn_goal - vn_current;
+
+      // Only apply if we need to add impulse
+      if (delta_vn <= 0) continue;
+
+      const wA = 1 / a.mass;
+      const wB = 1 / b.mass;
+      const wSum = wA + wB;
+      if (wSum === 0) continue;
+
+      const impulse = delta_vn / wSum;
+      a.vx -= impulse * wA * nx;
+      a.vy -= impulse * wA * ny;
+      b.vx += impulse * wB * nx;
+      b.vy += impulse * wB * ny;
     }
   }
 

src/utils/marbleSystem.ts

@@ -153,9 +153,9 @@ export class MarbleSystem {
       if (magnitude < 2.0) {
         subSteps = 1;
       } else if (magnitude < 5.0) {
-        subSteps = 2;
+        subSteps = 3;
       } else {
-        subSteps = 4;
+        subSteps = 6;
       }
 
       const maxMagnitude = 7.0;
@@ -192,7 +192,7 @@ export class MarbleSystem {
       // Update physics
       this.physics.updatePositions(this.marbles, subDt);
       this.physics.handleCollisions(this.marbles);
-      this.physics.handleBoundaries(this.marbles);
+      this.physics.resolveVelocities(this.marbles, subDt);
     }
 
     this.physics.render(this.marbles);