feat: xpbd 구현

src/pendulum/drawing.ts

@@ -86,6 +86,24 @@ export function drawWorld(ctx: CanvasRenderingContext2D,
         const b1 = world.balls[constraint.p1Idx];
         const b2 = world.balls[constraint.p2Idx];
         drawConstraint(ctx, b1, b2);
+        // Draw force vectors for debugging
+        const scale = 100; // Scale for visibility
+        if (constraint.p1ConstraintForce) {
+            ctx.beginPath();
+            ctx.moveTo(b1.pos[0], b1.pos[1]);
+            ctx.lineTo(b1.pos[0] + constraint.p1ConstraintForce[0] * scale, b1.pos[1] + constraint.p1ConstraintForce[1] * scale);
+            ctx.strokeStyle = "green";
+            ctx.lineWidth = 3;
+            ctx.stroke();
+        }
+        if (constraint.p2ConstraintForce) {
+            ctx.beginPath();
+            ctx.moveTo(b2.pos[0], b2.pos[1]);
+            ctx.lineTo(b2.pos[0] + constraint.p2ConstraintForce[0] * scale, b2.pos[1] + constraint.p2ConstraintForce[1] * scale);
+            ctx.strokeStyle = "green";
+            ctx.lineWidth = 3;
+            ctx.stroke();
+        }
     }
     for (let i = 0; i < world.balls.length; i++) {
         const ball = world.balls[i];

src/pendulum/mod.tsx

@@ -124,6 +124,11 @@ export default function Pendulum() {
   // reset handler to re-create pendulums and avoid large dt on next frame
   const reset = () => {
     worldRef.current = getDefaultMyWorld();
+    if (worldRef.current) {
+      for (const constraint of worldRef.current.constraints) {
+        constraint.lagrangeMultiplier = 0;
+      }
+    }
     prevTimeRef.current = performance.now();
   };
 

src/pendulum/physics.ts

@@ -13,7 +13,11 @@ export type ConstraintState = {
 	p1Idx: number;
 	p2Idx: number;
 	restLength: number;
-	stiffness: number;
+	compliance: number;
+	lagrangeMultiplier?: number;
+
+	p1ConstraintForce?: [number,number]; // For debugging or visualization
+	p2ConstraintForce?: [number,number]; // For debugging or visualization
 };
 
 export type PhysicalWorldState = {
@@ -21,11 +25,12 @@ export type PhysicalWorldState = {
 	constraints: ConstraintState[];
 };
 
-export const SCENE_GRAVITY: Point = [0, 9.81 * 100];
+export const SCENE_GRAVITY: Point = [0, 9.81 * 10];
 
 export function updateUnconstrainedBall(ball: BallState, dt: number = 0.016) {
 	if (ball.isFixed) return;
 	const acc: Point = [SCENE_GRAVITY[0], SCENE_GRAVITY[1]];
+	// A more exact derivation uses the Taylor series (to second order)
 	const nextPos: Point = [
 		ball.pos[0] + (ball.pos[0] - ball.prevPos[0]) * (dt / ball.prevDt) + acc[0] * (dt + ball.prevDt) / 2 * dt,
 		ball.pos[1] + (ball.pos[1] - ball.prevPos[1]) * (dt / ball.prevDt) + acc[1] * (dt + ball.prevDt) / 2 * dt,
@@ -35,26 +40,47 @@ export function updateUnconstrainedBall(ball: BallState, dt: number = 0.016) {
 	ball.prevDt = dt;
 }
 
-export function satisfyConstraint(ball1: BallState, ball2: BallState, constraint: ConstraintState) {
+// XPBD (Extended Position Based Dynamics) constraint solver
+export function satisfyConstraint(ball1: BallState, ball2: BallState, 
+	constraint: ConstraintState,
+	dt: number = 0.016
+) {
 	const dx = ball2.pos[0] - ball1.pos[0];
 	const dy = ball2.pos[1] - ball1.pos[1];
 	const length = Math.sqrt(dx * dx + dy * dy);
-	if (length == 0) return;
+	if (length < 1e-9) return;
-	const diff = length - constraint.restLength;
+	const C = (constraint.restLength - length);
+	const [nx, ny] = [dx / length, dy / length];
 	const invMass1 = 1 / ball1.mass;
 	const invMass2 = 1 / ball2.mass;
 	const sumInvMass = invMass1 + invMass2;
-	if (sumInvMass == 0) return;
+
-	const p1Prop = invMass1 / sumInvMass;
+	if (sumInvMass < 1e-9) return; // Both balls are fixed
-	const p2Prop = invMass2 / sumInvMass;
+
-	const correction = diff * constraint.stiffness / length;
+	const compliance = constraint.compliance ?? 0;
-	ball1.pos[0] += dx * correction * p1Prop;
+	const alpha = compliance / (dt * dt);
-	ball1.pos[1] += dy * correction * p1Prop;
+
-	ball2.pos[0] -= dx * correction * p2Prop;
+	constraint.lagrangeMultiplier = (constraint.lagrangeMultiplier ?? 0);
-	ball2.pos[1] -= dy * correction * p2Prop;
+
+	const deltaLambda = (-C - alpha * constraint.lagrangeMultiplier) / (sumInvMass + alpha);
+	constraint.lagrangeMultiplier += deltaLambda;
+
+	const correction = deltaLambda;
+
+	ball1.pos[0] += nx * correction * invMass1;
+	ball1.pos[1] += ny * correction * invMass1;
+	ball2.pos[0] -= nx * correction * invMass2;
+	ball2.pos[1] -= ny * correction * invMass2;
+
+	constraint.p1ConstraintForce = [nx * correction * invMass1 / dt, ny * correction * invMass1 / dt];
+	constraint.p2ConstraintForce = [-nx * correction * invMass2 / dt, -ny * correction * invMass2 / dt];
 }
 
 export function updateWorld(world: PhysicalWorldState, dt: number = 0.016, constraintIterations: number = 50) {
+	for (const constraint of world.constraints) {
+		constraint.lagrangeMultiplier = 0;
+	}
+	
 	for (const ball of world.balls) {
 		updateUnconstrainedBall(ball, dt);
 	}
@@ -62,7 +88,7 @@ export function updateWorld(world: PhysicalWorldState, dt: number = 0.016, const
 		for (const constraint of world.constraints) {
 			const b1 = world.balls[constraint.p1Idx];
 			const b2 = world.balls[constraint.p2Idx];
-			satisfyConstraint(b1, b2, constraint);
+			satisfyConstraint(b1, b2, constraint, dt);
 		}
 	}
 }

src/pendulum/worlds.ts

@@ -13,10 +13,10 @@ export function getDefaultMyWorld(): WorldState {
 		{ pos: [400, 0], prevPos: [400, 0], mass: 1, prevDt: 0.016 },
 	];
 	const constraints: ConstraintState[] = [
-		{ p1Idx: 0, p2Idx: 1, restLength: 100, stiffness: 1 },
+		{ p1Idx: 0, p2Idx: 1, restLength: 100, compliance: 0 },
-		{ p1Idx: 1, p2Idx: 2, restLength: 50, stiffness: 1 },
+		{ p1Idx: 1, p2Idx: 2, restLength: 50, compliance: 0.01 },
-		{ p1Idx: 0, p2Idx: 3, restLength: 200, stiffness: 1 },
+		{ p1Idx: 0, p2Idx: 3, restLength: 200, compliance: 0 },
-		{ p1Idx: 3, p2Idx: 4, restLength: 50, stiffness: 1 },
+		{ p1Idx: 3, p2Idx: 4, restLength: 50, compliance: 0 },
 	];
 	const ballDrawers = [
 		new BallDrawer({ radius: 10 }),