physics.js

/**
 * Master Physics Sub-stepping Integrator
 */

/**
 * Comprehensive NaN and undefined object sanitizer
 */
function sanitizeBody(obj) {
  if (!obj) return;
  if (!obj.pos || isNaN(obj.pos.x) || isNaN(obj.pos.y)) {
    obj.pos = { x: 5, y: 5 }; 
  }
  if (!obj.vel || isNaN(obj.vel.x) || isNaN(obj.vel.y)) {
    obj.vel = { x: 0, y: 0 };
  }
  if (!obj.force || isNaN(obj.force.x) || isNaN(obj.force.y)) {
    obj.force = { x: 0, y: 0 };
  }
  if (typeof obj.mass !== 'number' || isNaN(obj.mass) || obj.mass <= 0) obj.mass = 1;
  if (typeof obj.angle !== 'number' || isNaN(obj.angle)) obj.angle = 0;
  if (typeof obj.omega !== 'number' || isNaN(obj.omega)) obj.omega = 0;
}

/**
 * Resolves pairwise impulse collisions between dynamic & static bodies
 */
function resolveCollision(bodyA, bodyB) {
  if (!bodyA || !bodyB || bodyA === bodyB) return;

  const collidesA = bodyA.collides !== false && bodyA.enableCollisions !== false;
  const collidesB = bodyB.collides !== false && bodyB.enableCollisions !== false;
  if (!collidesA || !collidesB) return;

  if (bodyA.isAnchored && bodyB.isAnchored) return;

  // Get collision geometry data from a single unified helper
  const contact = getCollisionData(bodyA, bodyB);
  if (!contact) return; // No collision

  const { nx, ny, overlap } = contact;

  const invMassA = (!bodyA.isAnchored && !bodyA.isDragging) ? (1 / (bodyA.mass || 1)) : 0;
  const invMassB = (!bodyB.isAnchored && !bodyB.isDragging) ? (1 / (bodyB.mass || 1)) : 0;
  const totalInvMass = invMassA + invMassB;

  if (totalInvMass === 0) return;

  // 1. Positional Separation (Push apart)
  const posA = bodyA.pos;
  const posB = bodyB.pos;

  if (invMassA > 0) {
    posA.x -= nx * overlap * (invMassA / totalInvMass);
    posA.y -= ny * overlap * (invMassA / totalInvMass);
  }
  if (invMassB > 0) {
    posB.x += nx * overlap * (invMassB / totalInvMass);
    posB.y += ny * overlap * (invMassB / totalInvMass);
  }

  // 2. Velocity Impulse Response (Bounce)
  const velA = bodyA.vel || { x: 0, y: 0 };
  const velB = bodyB.vel || { x: 0, y: 0 };
  const relVelX = velB.x - velA.x;
  const relVelY = velB.y - velA.y;
  const velAlongNormal = relVelX * nx + relVelY * ny;

  if (velAlongNormal < 0) {
    // Calculate combined restitution (bounciness)
    const restA = bodyA.restitution !== undefined ? bodyA.restitution : 0.5;
    const restB = bodyB.restitution !== undefined ? bodyB.restitution : 0.5;
    let e = Math.min(restA, restB); 

    // FIX: Compensate for mass-splitting so dynamic-dynamic collisions 
    // match the punchiness of infinite-mass ground collisions.
    if (invMassA > 0 && invMassB > 0) {
      e = Math.min(1.0, e * 1.414); // Scales up dynamic restitution to balance energy sharing
    }

    const j = -(1 + e) * velAlongNormal / totalInvMass;

    if (invMassA > 0) {
      velA.x -= nx * j * invMassA;
      velA.y -= ny * j * invMassA;
    }
    if (invMassB > 0) {
      velB.x += nx * j * invMassB;
      velB.y += ny * j * invMassB;
    }
  }
}

function getCollisionData(bodyA, bodyB) {
  const isBoxA = !!bodyA.width;
  const isBoxB = !!bodyB.width;

  if (!isBoxA && !isBoxB) {
    // Circle vs Circle
    const dx = bodyB.pos.x - bodyA.pos.x;
    const dy = bodyB.pos.y - bodyA.pos.y;
    const dist = Math.hypot(dx, dy) || 0.0001;
    const minDist = (bodyA.radius || 0.2) + (bodyB.radius || 0.2);
    if (dist < minDist) {
      return { nx: dx / dist, ny: dy / dist, overlap: minDist - dist };
    }
  } else if (isBoxA && !isBoxB) {
    // Box vs Circle
    return getBoxCircleCollision(bodyA, bodyB);
  } else if (!isBoxA && isBoxB) {
    // Circle vs Box (invert result)
    const data = getBoxCircleCollision(bodyB, bodyA);
    if (data) {
      return { nx: -data.nx, ny: -data.ny, overlap: data.overlap };
    }
  } else {
    // Box vs Box (AABB)
    return getBoxBoxCollision(bodyA, bodyB);
  }
  return null;
}

function getBoxCircleCollision(box, circle) {
  const boxHalfW = box.width / 2;
  const boxHalfH = box.height / 2;

  const cx = circle.pos.x - box.pos.x;
  const cy = circle.pos.y - box.pos.y;

  const closestX = Math.max(-boxHalfW, Math.min(boxHalfW, cx));
  const closestY = Math.max(-boxHalfH, Math.min(boxHalfH, cy));

  const distX = cx - closestX;
  const distY = cy - closestY;
  const distance = Math.hypot(distX, distY);
  const minDist = circle.radius || 0.2;

  if (distance < minDist && distance > 0) {
    return { nx: distX / distance, ny: distY / distance, overlap: minDist - distance };
  }
  if (distance === 0) {
    return { nx: 1, ny: 0, overlap: minDist };
  }
  return null;
}

function getBoxBoxCollision(boxA, boxB) {
  const halfWA = boxA.width / 2;
  const halfHA = boxA.height / 2;
  const halfWB = boxB.width / 2;
  const halfHB = boxB.height / 2;

  const dx = boxB.pos.x - boxA.pos.x;
  const dy = boxB.pos.y - boxA.pos.y;

  const overlapX = (halfWA + halfWB) - Math.abs(dx);
  if (overlapX <= 0) return null;

  const overlapY = (halfHA + halfHB) - Math.abs(dy);
  if (overlapY <= 0) return null;

  if (overlapX < overlapY) {
    return { nx: dx > 0 ? 1 : -1, ny: 0, overlap: overlapX };
  } else {
    return { nx: 0, ny: dy > 0 ? 1 : -1, overlap: overlapY };
  }
}

/**
 * Master Physics Sub-stepping Integrator
 */
function stepPhysics(dt, ground, particles = [], springs = [], strings = [], boxes = [], pulleys = []) {
  const subSteps = 8;
  const subDt = dt / subSteps;
  const allDynamic = [...particles, ...boxes];
  
  const gravityAcc = parseFloat(document.getElementById('gravity-slider')?.value ?? 9.81);

  for (let s = 0; s < subSteps; s++) {
    // 1. Sanitize, Reset Forces & Apply Gravity
    for (let obj of allDynamic) {
      if (!obj) continue;
      
      sanitizeBody(obj); 

      obj.force.x = 0;
      obj.force.y = 0;
      obj._stringTensionForce = { x: 0, y: 0 };

      if (obj.isAnchored || obj.isDragging) continue;

      const m = obj.mass || 1;
      obj.force.y -= m * gravityAcc;
    }

    // 1.5 Apply Wind Forces
    if (typeof applyWindToAll === 'function') {
      applyWindToAll(allDynamic, 1.0);
    }

    // 1.55 Apply WASD force only to the selected object
    if (typeof applyAppliedForceToBody === 'function' && typeof getSelectedPhysicsObject === 'function') {
      applyAppliedForceToBody(getSelectedPhysicsObject());
    }

    // 1.6 Apply velocity-dependent air and fluid resistance
    if (typeof applyResistanceToAll === 'function') {
      applyResistanceToAll(allDynamic);
    }

    // 2. Solve Spring Hooke's Law Forces
    for (let sp of springs) {
      if (!sp) continue;
      const nodeA = sp.nodeA?.body || sp.nodeA;
      const nodeB = sp.nodeB?.body || sp.nodeB;
      if (!nodeA?.pos || !nodeB?.pos) continue;

      const dx = nodeB.pos.x - nodeA.pos.x;
      const dy = nodeB.pos.y - nodeA.pos.y;
      const dist = Math.hypot(dx, dy);

      const restLength = sp.restLength || 1.5;
      const k = sp.k || 50;        
      const damping = sp.damping || 0.2;

      const delta = dist - restLength;
      const nx = dist > 0 ? dx / dist : 0;
      const ny = dist > 0 ? dy / dist : 0;

      const velA = nodeA.vel || { x: 0, y: 0 };
      const velB = nodeB.vel || { x: 0, y: 0 };
      const relVelX = velB.x - velA.x;
      const relVelY = velB.y - velA.y;

      const safeDelta = Math.max(-2.0, Math.min(delta, 2.0));
      let springForceMag = (k * safeDelta) + ((relVelX * nx + relVelY * ny) * damping);
      springForceMag = Math.max(-5000, Math.min(springForceMag, 5000));

      if (!nodeA.isAnchored && !nodeA.isDragging && nodeA.force) {
        nodeA.force.x += nx * springForceMag;
        nodeA.force.y += ny * springForceMag;
      }
      if (!nodeB.isAnchored && !nodeB.isDragging && nodeB.force) {
        nodeB.force.x -= nx * springForceMag;
        nodeB.force.y -= ny * springForceMag;
      }
    }

    // 2.5 String constraints are now handled ONLY by solveStringConstraints in solver.js
    // Removed duplicate tension forces and position corrections to prevent over-constraining

    // 3. Euler Integration & Ground Collision
    for (let obj of allDynamic) {
      if (!obj || obj.isAnchored || obj.isDragging) continue;

      const m = obj.mass || 1;
      const ax = obj.force.x / m;
      const ay = obj.force.y / m;

      obj.vel.x += ax * subDt;
      obj.vel.y += ay * subDt;

      const maxSpeed = 50.0;
      const speed = Math.hypot(obj.vel.x, obj.vel.y);
      if (speed > maxSpeed || isNaN(speed)) {
        obj.vel.x = isNaN(speed) ? 0 : (obj.vel.x / speed) * maxSpeed;
        obj.vel.y = isNaN(speed) ? 0 : (obj.vel.y / speed) * maxSpeed;
      }

      obj.pos.x += obj.vel.x * subDt;
      obj.pos.y += obj.vel.y * subDt;

      if (typeof obj.omega === 'number') {
        if (typeof obj.angle !== 'number') obj.angle = 0;
        obj.angle += obj.omega * subDt;
        obj.omega *= 0.995;
      }

      if (ground && ground.y !== undefined) {
        let halfH = obj.radius || 0.2;
        const isBox = typeof obj.width === 'number' && typeof obj.height === 'number';

        if (isBox) {
          const angle = obj.angle || 0;
          const halfW = obj.width / 2;
          const hH = obj.height / 2;
          halfH = Math.abs(halfW * Math.sin(angle)) + Math.abs(hH * Math.cos(angle));
        }

        if (obj.pos.y - halfH <= ground.y) {
          obj.pos.y = ground.y + halfH;

          if (obj.vel.y < 0) {
            const restObj = obj.restitution !== undefined ? obj.restitution : 0.5;
            const restGround = ground.restitution !== undefined ? ground.restitution : 0.3;
            const e = Math.min(restObj, restGround);

            obj.vel.y = -obj.vel.y * e;
            if (Math.abs(obj.vel.y) < 0.05) obj.vel.y = 0;
          }

          const objectFriction = obj.friction ?? 0.3;
          const surfaceFriction = ground.friction ?? 0.3;
          const friction = Math.min(1, (objectFriction + surfaceFriction) / 2);
          obj.vel.x *= (1 - friction);

          if (isBox && typeof obj.omega === 'number') {
            const sinAngle = Math.sin(obj.angle || 0);
            const restoringTorque = -sinAngle * 25.0; 
            
            obj.omega += restoringTorque * subDt;
            obj.omega *= 0.85;

            if (Math.abs(obj.angle) < 0.05) {
              obj.angle = 0;
              obj.omega = 0;
            }
          } else if (typeof obj.omega === 'number') {
            const r = obj.radius || halfH;
            obj.omega += (obj.vel.x / r) * (obj.rollingFriction ?? ground.rollingFriction ?? 0.1);
            obj.omega *= (1 - friction);
          }
        }
      }
    }

    // 4. Multi-body Pairwise Object Collisions
    const allColliders = [...particles, ...boxes, ...pulleys].filter(Boolean);
    for (let i = 0; i < allColliders.length; i++) {
      for (let j = i + 1; j < allColliders.length; j++) {
        if (typeof resolveCollision === 'function') {
          resolveCollision(allColliders[i], allColliders[j]);
        }
      }
    }

    // 4.5 Solve String & Pulley Constraints
    if (typeof solveStringConstraints === 'function') {
      solveStringConstraints(strings, ground?.y);
    }

    // 5. Post-Collision Velocity Failsafe
    for (let obj of allDynamic) {
      if (!obj || obj.isAnchored || obj.isDragging) continue;
      const speed = Math.hypot(obj.vel.x, obj.vel.y);
      if (speed > 50.0 || isNaN(speed)) {
        obj.vel.x = isNaN(speed) ? 0 : (obj.vel.x / speed) * 50.0;
        obj.vel.y = isNaN(speed) ? 0 : (obj.vel.y / speed) * 50.0;
      }
    }
  }
}