@aura3d/physics 1.4.4 → 1.5.0

package/dist/PhysicsWorld.js

@@ -2,9 +2,11 @@ import { Body as CannonBody, Box as CannonBox, Cylinder as CannonCylinder, Plane
 import { Collider } from "./Collider.js";
 import { CollisionEventQueue } from "./CollisionEvents.js";
 import { Constraint } from "./Constraint.js";
+import { buildNativeNarrowPhaseContact } from "./NarrowPhase.js";
 import { raycastCollider, sphereCastCollider } from "./Raycast.js";
 import { RigidBody } from "./RigidBody.js";
-import { cloneVec3, dotVec3, normalizeVec3, scaleVec3, subVec3 } from "./Shape.js";
+import { cloneVec3, dotVec3, lengthVec3, normalizeVec3, scaleVec3, subVec3 } from "./Shape.js";
+import { timeOfImpact } from "./TimeOfImpact.js";
 export class PhysicsWorld {
     gravity;
     fixedDelta;
@@ -12,6 +14,7 @@ export class PhysicsWorld {
     enableSleeping;
     sleepVelocityThreshold;
     sleepDelay;
+    continuousCollision;
     bodiesById = new Map();
     collidersById = new Map();
     constraintsList = [];
@@ -26,6 +29,7 @@ export class PhysicsWorld {
     lastEvents = [];
     lastBroadphasePairs = 0;
     lastBroadphaseProfile = emptyBroadphaseProfile();
+    lastContinuousCollisionStep = emptyContinuousCollisionStepPlan();
     steps = 0;
     constructor(descriptor = {}) {
         this.requestedBackend = descriptor.backend ?? "auto";
@@ -35,6 +39,7 @@ export class PhysicsWorld {
         this.enableSleeping = descriptor.enableSleeping ?? true;
         this.sleepVelocityThreshold = descriptor.sleepVelocityThreshold ?? 0.02;
         this.sleepDelay = descriptor.sleepDelay ?? 0.5;
+        this.continuousCollision = normalizeContinuousCollisionDescriptor(descriptor.continuousCollision);
         if (!Number.isFinite(this.fixedDelta) || this.fixedDelta <= 0) {
             throw new Error("fixedDelta must be a finite positive number.");
         }
@@ -51,7 +56,8 @@ export class PhysicsWorld {
             requested: this.requestedBackend,
             active: this.requestedBackend === "aura-js" ? "aura-js" : "cannon-es",
             deterministic: true,
-            jsFallbackAvailable: true
+            jsFallbackAvailable: true,
+            continuousCollision: this.continuousCollisionSelection(true)
         };
         if (this.backendSelection.active === "cannon-es") {
             this.cannonWorld = new CannonWorld({
@@ -154,21 +160,43 @@ export class PhysicsWorld {
         if (this.cannonWorld) {
             return this.stepCannon(dt);
         }
-        for (const body of this.bodyValues()) {
-            body.integrate(dt, this.gravity);
-        }
-        for (const constraint of this.constraintsList) {
-            constraint.solve();
-        }
+        const continuousStep = this.planContinuousCollisionStep(dt);
+        this.lastContinuousCollisionStep = continuousStep;
+        this.assertContinuousCollisionPlan(continuousStep);
+        const subDelta = dt / continuousStep.subSteps;
+        const outerStepTransforms = new Map(this.bodies().map((body) => [
+            body.id,
+            {
+                position: cloneVec3(body.position),
+                rotation: [...body.rotation]
+            }
+        ]));
         let contacts = [];
-        for (let i = 0; i < this.solverIterations; i += 1) {
-            contacts = this.detectContacts();
-            for (const contact of contacts) {
-                this.resolveContact(contact);
+        for (let subStep = 0; subStep < continuousStep.subSteps; subStep += 1) {
+            for (const body of this.bodyValues()) {
+                body.integrate(subDelta, this.gravity, false);
             }
             for (const constraint of this.constraintsList) {
                 constraint.solve();
             }
+            const contactImpulses = new Map();
+            for (let i = 0; i < this.solverIterations; i += 1) {
+                contacts = this.detectContacts();
+                for (const contact of contacts) {
+                    this.resolveContact(contact, contactImpulses);
+                }
+                for (const constraint of this.constraintsList) {
+                    constraint.solve();
+                }
+            }
+        }
+        for (const body of this.bodyValues()) {
+            body.clearForces();
+            const outerTransform = outerStepTransforms.get(body.id);
+            if (outerTransform) {
+                body.previousPosition = outerTransform.position;
+                body.previousRotation = outerTransform.rotation;
+            }
         }
         this.lastEvents = this.eventQueue.update(contacts);
         this.updateSleeping(dt, contacts);
@@ -223,7 +251,10 @@ export class PhysicsWorld {
         const contacts = this.eventQueue.snapshotContacts();
         const bodies = this.bodies();
         return {
-            backend: this.backendSelection,
+            backend: {
+                ...this.backendSelection,
+                continuousCollision: this.continuousCollisionSelection(true)
+            },
             bodies: bodies.map((body) => body.snapshot()),
             contacts,
             stats: {
@@ -267,7 +298,7 @@ export class PhysicsWorld {
             if (!body) {
                 continue;
             }
-            const bounds = collider.bounds(body.position);
+            const bounds = collider.bounds(body.position, body.rotation);
             const entry = { collider, body, bounds };
             if (isFiniteBounds(bounds)) {
                 finiteEntries.push(entry);
@@ -314,7 +345,7 @@ export class PhysicsWorld {
         this.lastBroadphaseProfile = profile;
         return pairs;
     }
-    resolveContact(contact) {
+    resolveContact(contact, contactImpulses) {
         if (contact.sensor) {
             return;
         }
@@ -334,30 +365,46 @@ export class PhysicsWorld {
         if (bodyB.inverseMass > 0) {
             bodyB.position = [bodyB.position[0] + correction[0] * bodyB.inverseMass, bodyB.position[1] + correction[1] * bodyB.inverseMass, bodyB.position[2] + correction[2] * bodyB.inverseMass];
         }
-        const relativeVelocity = subVec3(bodyB.velocity, bodyA.velocity);
+        const relativeVelocity = relativeContactVelocity(bodyA, bodyB, contact.point);
         const velocityAlongNormal = dotVec3(relativeVelocity, contact.normal);
         const materialA = this.effectiveMaterial(bodyA, contact.colliderA);
         const materialB = this.effectiveMaterial(bodyB, contact.colliderB);
         const restitution = Math.max(materialA.restitution, materialB.restitution);
+        const pairKey = `${contact.colliderA}:${contact.colliderB}`;
+        const impulseState = contactImpulses.get(pairKey) ?? { normal: 0, tangent: [0, 0, 0] };
         let normalImpulseMagnitude = 0;
         if (velocityAlongNormal <= 0) {
-            normalImpulseMagnitude = -(1 + restitution) * velocityAlongNormal / invMassSum;
-            this.applyImpulsePair(bodyA, bodyB, scaleVec3(contact.normal, normalImpulseMagnitude));
+            const normalDenominator = contactImpulseDenominator(bodyA, bodyB, contact.normal, contact.point);
+            normalImpulseMagnitude = normalDenominator > 0 ? -(1 + restitution) * velocityAlongNormal / normalDenominator : 0;
+            impulseState.normal += normalImpulseMagnitude;
+            this.applyImpulsePair(bodyA, bodyB, scaleVec3(contact.normal, normalImpulseMagnitude), contact.point);
         }
-        const updatedRelativeVelocity = subVec3(bodyB.velocity, bodyA.velocity);
+        const updatedRelativeVelocity = relativeContactVelocity(bodyA, bodyB, contact.point);
         const updatedNormalVelocity = dotVec3(updatedRelativeVelocity, contact.normal);
         const tangentVelocity = subVec3(updatedRelativeVelocity, scaleVec3(contact.normal, updatedNormalVelocity));
         const tangentSpeed = Math.hypot(tangentVelocity[0], tangentVelocity[1], tangentVelocity[2]);
         if (tangentSpeed > 1e-9) {
-            const tangent = scaleVec3(tangentVelocity, 1 / tangentSpeed);
             const friction = Math.sqrt(Math.max(0, materialA.friction) * Math.max(0, materialB.friction));
-            const targetMagnitude = -dotVec3(updatedRelativeVelocity, tangent) / invMassSum;
-            const maxFriction = friction * (Math.abs(normalImpulseMagnitude) + contact.penetration);
-            const frictionMagnitude = Math.max(-maxFriction, Math.min(maxFriction, targetMagnitude));
-            if (frictionMagnitude !== 0) {
-                this.applyImpulsePair(bodyA, bodyB, scaleVec3(tangent, frictionMagnitude));
+            const tangent = scaleVec3(tangentVelocity, 1 / tangentSpeed);
+            const tangentDenominator = contactImpulseDenominator(bodyA, bodyB, tangent, contact.point);
+            const targetImpulse = tangentDenominator > 0 ? scaleVec3(tangentVelocity, -1 / tangentDenominator) : [0, 0, 0];
+            const proposedTangent = [
+                impulseState.tangent[0] + targetImpulse[0],
+                impulseState.tangent[1] + targetImpulse[1],
+                impulseState.tangent[2] + targetImpulse[2]
+            ];
+            const proposedMagnitude = lengthVec3(proposedTangent);
+            const maxFriction = friction * impulseState.normal;
+            const accumulatedTangent = proposedMagnitude > maxFriction && proposedMagnitude > 0
+                ? scaleVec3(proposedTangent, maxFriction / proposedMagnitude)
+                : proposedTangent;
+            const frictionImpulse = subVec3(accumulatedTangent, impulseState.tangent);
+            impulseState.tangent = accumulatedTangent;
+            if (lengthVec3(frictionImpulse) > 0) {
+                this.applyImpulsePair(bodyA, bodyB, frictionImpulse, contact.point);
             }
         }
+        contactImpulses.set(pairKey, impulseState);
     }
     effectiveMaterial(body, colliderId) {
         const collider = this.collidersById.get(colliderId);
@@ -366,15 +413,21 @@ export class PhysicsWorld {
             friction: Math.max(body.friction, collider?.material.friction ?? 0)
         };
     }
-    applyImpulsePair(bodyA, bodyB, impulse) {
+    applyImpulsePair(bodyA, bodyB, impulse, point) {
         if (bodyA.inverseMass > 0) {
-            bodyA.velocity = [bodyA.velocity[0] - impulse[0] * bodyA.inverseMass, bodyA.velocity[1] - impulse[1] * bodyA.inverseMass, bodyA.velocity[2] - impulse[2] * bodyA.inverseMass];
+            if (point)
+                bodyA.applyContactImpulseAtPoint(scaleVec3(impulse, -1), point);
+            else
+                bodyA.velocity = [bodyA.velocity[0] - impulse[0] * bodyA.inverseMass, bodyA.velocity[1] - impulse[1] * bodyA.inverseMass, bodyA.velocity[2] - impulse[2] * bodyA.inverseMass];
             if (bodyA.sleeping && bodyA.speedSquared() > this.sleepVelocityThreshold * this.sleepVelocityThreshold) {
                 bodyA.wake();
             }
         }
         if (bodyB.inverseMass > 0) {
-            bodyB.velocity = [bodyB.velocity[0] + impulse[0] * bodyB.inverseMass, bodyB.velocity[1] + impulse[1] * bodyB.inverseMass, bodyB.velocity[2] + impulse[2] * bodyB.inverseMass];
+            if (point)
+                bodyB.applyContactImpulseAtPoint(impulse, point);
+            else
+                bodyB.velocity = [bodyB.velocity[0] + impulse[0] * bodyB.inverseMass, bodyB.velocity[1] + impulse[1] * bodyB.inverseMass, bodyB.velocity[2] + impulse[2] * bodyB.inverseMass];
             if (bodyB.sleeping && bodyB.speedSquared() > this.sleepVelocityThreshold * this.sleepVelocityThreshold) {
                 bodyB.wake();
             }
@@ -460,7 +513,8 @@ export class PhysicsWorld {
             active: "aura-js",
             fallback: reason,
             deterministic: true,
-            jsFallbackAvailable: true
+            jsFallbackAvailable: true,
+            continuousCollision: this.continuousCollisionSelection(true)
         };
     }
     stepCannon(dt) {
@@ -472,7 +526,14 @@ export class PhysicsWorld {
             if (cannonBody)
                 syncCannonFromAura(body, cannonBody);
         }
-        this.cannonWorld.step(dt);
+        const continuousStep = this.planContinuousCollisionStep(dt);
+        this.lastContinuousCollisionStep = continuousStep;
+        this.assertContinuousCollisionPlan(continuousStep);
+        const subSteps = continuousStep.subSteps;
+        const subDelta = dt / subSteps;
+        for (let index = 0; index < subSteps; index += 1) {
+            this.cannonWorld.step(subDelta);
+        }
         for (const body of this.bodyValues()) {
             const cannonBody = this.cannonBodiesByAuraId.get(body.id);
             if (cannonBody)
@@ -484,6 +545,167 @@ export class PhysicsWorld {
         this.steps += 1;
         return this.lastEvents;
     }
+    planContinuousCollisionStep(dt) {
+        if (!this.continuousCollision.active) {
+            return emptyContinuousCollisionStepPlan();
+        }
+        let maxMotion = 0;
+        for (const body of this.bodyValues()) {
+            if (body.type === "static" || body.sleeping)
+                continue;
+            const bodyRadius = this.bodyBoundingRadius(body.id);
+            const linearMotion = lengthVec3(body.velocity) * dt;
+            const angularMotion = lengthVec3(body.angularVelocity) * bodyRadius * dt;
+            maxMotion = Math.max(maxMotion, linearMotion + angularMotion);
+        }
+        const minColliderFeature = this.smallestFiniteColliderFeature();
+        const earliestImpact = this.earliestTimeOfImpact(dt);
+        if (maxMotion <= 0 || minColliderFeature === undefined) {
+            return {
+                subSteps: 1,
+                requiredSubSteps: 1,
+                maxMotion,
+                minColliderFeature,
+                timeOfImpact: earliestImpact,
+                limitExceeded: false
+            };
+        }
+        const maxMotionPerSubstep = minColliderFeature * this.continuousCollision.motionThreshold;
+        const requiredSubSteps = Math.max(1, Math.ceil(maxMotion / maxMotionPerSubstep));
+        const limitExceeded = requiredSubSteps > this.continuousCollision.maxSubSteps;
+        return {
+            subSteps: Math.min(requiredSubSteps, this.continuousCollision.maxSubSteps),
+            requiredSubSteps,
+            maxMotion,
+            minColliderFeature,
+            timeOfImpact: earliestImpact,
+            limitExceeded
+        };
+    }
+    earliestTimeOfImpact(dt) {
+        let earliest;
+        const colliders = this.colliders();
+        for (let firstIndex = 0; firstIndex < colliders.length; firstIndex += 1) {
+            const colliderA = colliders[firstIndex];
+            const bodyA = this.bodiesById.get(colliderA.bodyId);
+            if (!bodyA)
+                continue;
+            for (let secondIndex = firstIndex + 1; secondIndex < colliders.length; secondIndex += 1) {
+                const colliderB = colliders[secondIndex];
+                const bodyB = this.bodiesById.get(colliderB.bodyId);
+                if (!bodyB ||
+                    bodyA.id === bodyB.id ||
+                    (bodyA.type !== "dynamic" && bodyB.type !== "dynamic") ||
+                    !colliderA.canCollideWith(colliderB)) {
+                    continue;
+                }
+                const impact = timeOfImpact(colliderA.shape, bodyA.position, bodyA.velocity, colliderB.shape, bodyB.position, bodyB.velocity, dt);
+                if (impact && (earliest === undefined || impact.time < earliest)) {
+                    earliest = impact.time;
+                }
+            }
+        }
+        return earliest;
+    }
+    assertContinuousCollisionPlan(plan) {
+        if (plan.limitExceeded && this.continuousCollision.onSubstepLimit === "error") {
+            throw new Error(`PhysicsWorld adaptive-substep CCD requires ${plan.requiredSubSteps} substeps, ` +
+                `above maxSubSteps ${this.continuousCollision.maxSubSteps}. Increase maxSubSteps, reduce dt, ` +
+                "or use a swept movement query; the step was rejected so tunneling is not silently accepted.");
+        }
+    }
+    smallestFiniteColliderFeature() {
+        let smallest;
+        for (const collider of this.colliderValues()) {
+            const feature = colliderFeatureSize(collider.shape);
+            if (feature === undefined)
+                continue;
+            smallest = smallest === undefined ? feature : Math.min(smallest, feature);
+        }
+        return smallest;
+    }
+    bodyBoundingRadius(bodyId) {
+        let radius = 0;
+        for (const colliderId of this.bodyColliders.get(bodyId) ?? []) {
+            const collider = this.collidersById.get(colliderId);
+            if (collider)
+                radius = Math.max(radius, colliderBoundingRadius(collider.shape));
+        }
+        return radius;
+    }
+    continuousCollisionSelection(backendSupportsWrapper) {
+        const active = this.continuousCollision.active && backendSupportsWrapper;
+        return {
+            active,
+            mode: active ? "adaptive-substeps" : "disabled",
+            provider: active ? "aura3d-adaptive-substep-wrapper" : "none",
+            maxSubSteps: this.continuousCollision.maxSubSteps,
+            motionThreshold: this.continuousCollision.motionThreshold,
+            onSubstepLimit: this.continuousCollision.onSubstepLimit,
+            lastSubSteps: active ? this.lastContinuousCollisionStep.subSteps : 1,
+            lastRequiredSubSteps: active ? this.lastContinuousCollisionStep.requiredSubSteps : 1,
+            lastMaxMotion: active ? this.lastContinuousCollisionStep.maxMotion : 0,
+            ...(active && this.lastContinuousCollisionStep.minColliderFeature !== undefined
+                ? { lastMinColliderFeature: this.lastContinuousCollisionStep.minColliderFeature }
+                : {}),
+            ...(active && this.lastContinuousCollisionStep.timeOfImpact !== undefined
+                ? { lastTimeOfImpact: this.lastContinuousCollisionStep.timeOfImpact }
+                : {}),
+            limitExceeded: active && this.lastContinuousCollisionStep.limitExceeded
+        };
+    }
+}
+function normalizeContinuousCollisionDescriptor(descriptor) {
+    if (!descriptor) {
+        return {
+            active: false,
+            maxSubSteps: 1,
+            motionThreshold: 0.5,
+            onSubstepLimit: "error"
+        };
+    }
+    const maxSubSteps = descriptor.maxSubSteps ?? 256;
+    const motionThreshold = descriptor.motionThreshold ?? 0.5;
+    const onSubstepLimit = descriptor.onSubstepLimit ?? "error";
+    if (!Number.isInteger(maxSubSteps) || maxSubSteps < 1) {
+        throw new Error("continuousCollision.maxSubSteps must be a positive integer.");
+    }
+    if (!Number.isFinite(motionThreshold) || motionThreshold <= 0 || motionThreshold > 1) {
+        throw new Error("continuousCollision.motionThreshold must be finite and in the range (0, 1].");
+    }
+    return {
+        active: true,
+        maxSubSteps,
+        motionThreshold,
+        onSubstepLimit
+    };
+}
+function emptyContinuousCollisionStepPlan() {
+    return {
+        subSteps: 1,
+        requiredSubSteps: 1,
+        maxMotion: 0,
+        limitExceeded: false
+    };
+}
+function colliderFeatureSize(shape) {
+    if (shape.kind === "box")
+        return Math.min(shape.halfExtents[0], shape.halfExtents[1], shape.halfExtents[2]);
+    if (shape.kind === "sphere" || shape.kind === "capsule")
+        return shape.radius;
+    return undefined;
+}
+function colliderBoundingRadius(shape) {
+    if (shape.kind === "box")
+        return Math.hypot(shape.halfExtents[0], shape.halfExtents[1], shape.halfExtents[2]);
+    if (shape.kind === "sphere")
+        return shape.radius;
+    if (shape.kind === "capsule")
+        return shape.radius + shape.halfHeight;
+    if (shape.kind === "mesh") {
+        return shape.vertices.reduce((radius, vertex) => Math.max(radius, lengthVec3(vertex)), 0);
+    }
+    return 0;
 }
 function toCannonVec3(value) {
     return new CannonVec3(value[0], value[1], value[2]);
@@ -543,6 +765,41 @@ function totalKineticEnergy(bodies) {
     }
     return total;
 }
+function relativeContactVelocity(bodyA, bodyB, point) {
+    if (!point)
+        return subVec3(bodyB.velocity, bodyA.velocity);
+    const radiusA = subVec3(point, bodyA.position);
+    const radiusB = subVec3(point, bodyB.position);
+    const velocityA = addVectors(bodyA.velocity, crossVectors(bodyA.angularVelocity, radiusA));
+    const velocityB = addVectors(bodyB.velocity, crossVectors(bodyB.angularVelocity, radiusB));
+    return subVec3(velocityB, velocityA);
+}
+function contactImpulseDenominator(bodyA, bodyB, direction, point) {
+    let denominator = bodyA.inverseMass + bodyB.inverseMass;
+    if (!point)
+        return denominator;
+    if (bodyA.inverseMass > 0) {
+        const radius = subVec3(point, bodyA.position);
+        const angular = bodyA.multiplyInverseInertiaWorld(crossVectors(radius, direction));
+        denominator += dotVec3(direction, crossVectors(angular, radius));
+    }
+    if (bodyB.inverseMass > 0) {
+        const radius = subVec3(point, bodyB.position);
+        const angular = bodyB.multiplyInverseInertiaWorld(crossVectors(radius, direction));
+        denominator += dotVec3(direction, crossVectors(angular, radius));
+    }
+    return denominator;
+}
+function addVectors(a, b) {
+    return [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
+}
+function crossVectors(a, b) {
+    return [
+        a[1] * b[2] - a[2] * b[1],
+        a[2] * b[0] - a[0] * b[2],
+        a[0] * b[1] - a[1] * b[0]
+    ];
+}
 function emptyBroadphaseProfile() {
     return {
         finiteColliders: 0,
@@ -634,6 +891,19 @@ function buildContact(a, bodyA, b, bodyB) {
     if (a.shape.kind === "capsule" && b.shape.kind === "capsule") {
         return buildCapsuleCapsuleContact(a, bodyA, b, bodyB);
     }
+    const nativeContact = buildNativeNarrowPhaseContact(a, bodyA, b, bodyB);
+    if (nativeContact) {
+        return {
+            colliderA: a.id,
+            colliderB: b.id,
+            bodyA: bodyA.id,
+            bodyB: bodyB.id,
+            normal: nativeContact.normal,
+            penetration: nativeContact.penetration,
+            point: nativeContact.point,
+            sensor: a.sensor || b.sensor
+        };
+    }
     const boundsA = a.bounds(bodyA.position);
     const boundsB = b.bounds(bodyB.position);
     const overlapX = Math.min(boundsA.max[0] - boundsB.min[0], boundsB.max[0] - boundsA.min[0]);
@@ -863,6 +1133,11 @@ function supportRadius(collider) {
             const bounds = collider.bounds([0, 0, 0]);
             return Math.max(Math.abs(bounds.min[1]), Math.abs(bounds.max[1]));
         }
+        case "convex-hull":
+        case "heightfield": {
+            const bounds = collider.bounds([0, 0, 0]);
+            return Math.max(Math.abs(bounds.min[1]), Math.abs(bounds.max[1]));
+        }
     }
 }
 //# sourceMappingURL=PhysicsWorld.js.map