import { addVec3, cloneVec3, scaleVec3, validateFiniteVec3, vec3 } from "./Shape.js";
export class RigidBody {
    id;
    type;
    mass;
    inverseMass;
    restitution;
    friction;
    position;
    previousPosition;
    rotation;
    previousRotation;
    velocity;
    angularVelocity;
    linearDamping;
    angularDamping;
    inverseInertia;
    sleeping;
    accumulatedForce;
    accumulatedTorque;
    sleepTimer = 0;
    constructor(id, descriptor = {}) {
        if (!Number.isInteger(id) || id <= 0) {
            throw new Error("RigidBody id must be a positive integer.");
        }
        this.id = id;
        this.type = descriptor.type ?? "dynamic";
        this.position = cloneVec3(descriptor.position ?? vec3());
        this.previousPosition = cloneVec3(this.position);
        this.rotation = normalizeQuat(descriptor.rotation ?? [0, 0, 0, 1]);
        this.previousRotation = cloneQuat(this.rotation);
        this.velocity = cloneVec3(descriptor.velocity ?? vec3());
        this.angularVelocity = cloneVec3(descriptor.angularVelocity ?? vec3());
        this.linearDamping = descriptor.linearDamping ?? 0;
        this.angularDamping = descriptor.angularDamping ?? 0;
        this.restitution = descriptor.restitution ?? 0;
        this.friction = descriptor.friction ?? 0.5;
        this.sleeping = descriptor.sleeping ?? false;
        validateFiniteVec3(this.position, "body position");
        validateFiniteVec3(this.velocity, "body velocity");
        validateFiniteVec3(this.angularVelocity, "body angularVelocity");
        if (!Number.isFinite(this.linearDamping) || this.linearDamping < 0) {
            throw new Error("linearDamping must be finite and non-negative.");
        }
        if (!Number.isFinite(this.angularDamping) || this.angularDamping < 0) {
            throw new Error("angularDamping must be finite and non-negative.");
        }
        const mass = descriptor.mass ?? 1;
        if (this.type === "dynamic") {
            if (!Number.isFinite(mass) || mass <= 0) {
                throw new Error("dynamic body mass must be a finite positive number.");
            }
            this.mass = mass;
            this.inverseMass = 1 / mass;
            this.inverseInertia = inverseInertia(descriptor.inertia ?? [mass, mass, mass]);
        }
        else {
            this.mass = Number.POSITIVE_INFINITY;
            this.inverseMass = 0;
            this.inverseInertia = vec3();
        }
        this.accumulatedForce = vec3();
        this.accumulatedTorque = vec3();
    }
    setPosition(position) {
        validateFiniteVec3(position, "body position");
        this.previousPosition = cloneVec3(this.position);
        this.position = cloneVec3(position);
        this.wake();
    }
    setVelocity(velocity) {
        validateFiniteVec3(velocity, "body velocity");
        this.velocity = cloneVec3(velocity);
        this.wake();
    }
    setRotation(rotation) {
        this.previousRotation = cloneQuat(this.rotation);
        this.rotation = normalizeQuat(rotation);
        this.wake();
    }
    setAngularVelocity(angularVelocity) {
        validateFiniteVec3(angularVelocity, "body angularVelocity");
        this.angularVelocity = cloneVec3(angularVelocity);
        this.wake();
    }
    applyForce(force) {
        validateFiniteVec3(force, "force");
        if (this.type !== "dynamic") {
            return;
        }
        this.accumulatedForce = addVec3(this.accumulatedForce, force);
        this.wake();
    }
    applyTorque(torque) {
        validateFiniteVec3(torque, "torque");
        if (this.type !== "dynamic") {
            return;
        }
        this.accumulatedTorque = addVec3(this.accumulatedTorque, torque);
        this.wake();
    }
    applyImpulse(impulse) {
        validateFiniteVec3(impulse, "impulse");
        if (this.type !== "dynamic") {
            return;
        }
        this.velocity = addVec3(this.velocity, scaleVec3(impulse, this.inverseMass));
        this.wake();
    }
    applyAngularImpulse(impulse) {
        validateFiniteVec3(impulse, "angular impulse");
        if (this.type !== "dynamic") {
            return;
        }
        this.angularVelocity = addVec3(this.angularVelocity, multiplyVec3(impulse, this.inverseInertia));
        this.wake();
    }
    applyImpulseAtPoint(impulse, worldPoint) {
        validateFiniteVec3(impulse, "impulse");
        validateFiniteVec3(worldPoint, "impulse point");
        if (this.type !== "dynamic") {
            return;
        }
        this.applyImpulse(impulse);
        this.applyAngularImpulse(crossVec3(subVec3(worldPoint, this.position), impulse));
    }
    integrate(dt, gravity) {
        if (!Number.isFinite(dt) || dt <= 0) {
            throw new Error("dt must be a finite positive number.");
        }
        if (this.type !== "dynamic" || this.sleeping) {
            this.clearForces();
            this.previousPosition = cloneVec3(this.position);
            this.previousRotation = cloneQuat(this.rotation);
            return;
        }
        this.previousPosition = cloneVec3(this.position);
        this.previousRotation = cloneQuat(this.rotation);
        const acceleration = addVec3(gravity, scaleVec3(this.accumulatedForce, this.inverseMass));
        this.velocity = addVec3(this.velocity, scaleVec3(acceleration, dt));
        this.angularVelocity = addVec3(this.angularVelocity, scaleVec3(multiplyVec3(this.accumulatedTorque, this.inverseInertia), dt));
        const dampingFactor = Math.max(0, 1 - this.linearDamping * dt);
        const angularDampingFactor = Math.max(0, 1 - this.angularDamping * dt);
        this.velocity = scaleVec3(this.velocity, dampingFactor);
        this.angularVelocity = scaleVec3(this.angularVelocity, angularDampingFactor);
        this.position = addVec3(this.position, scaleVec3(this.velocity, dt));
        this.rotation = integrateRotation(this.rotation, this.angularVelocity, dt);
        this.clearForces();
    }
    clearForces() {
        this.accumulatedForce = vec3();
        this.accumulatedTorque = vec3();
    }
    speedSquared() {
        return this.velocity[0] * this.velocity[0] + this.velocity[1] * this.velocity[1] + this.velocity[2] * this.velocity[2] +
            this.angularVelocity[0] * this.angularVelocity[0] + this.angularVelocity[1] * this.angularVelocity[1] + this.angularVelocity[2] * this.angularVelocity[2];
    }
    wake() {
        if (this.type !== "dynamic") {
            return;
        }
        this.sleeping = false;
        this.sleepTimer = 0;
    }
    sleep() {
        if (this.type !== "dynamic") {
            return;
        }
        this.velocity = vec3();
        this.angularVelocity = vec3();
        this.clearForces();
        this.sleeping = true;
    }
    resetSleepTimer() {
        this.sleepTimer = 0;
    }
    accumulateSleepTime(dt) {
        if (!Number.isFinite(dt) || dt < 0) {
            throw new Error("sleep dt must be finite and non-negative.");
        }
        this.sleepTimer += dt;
        return this.sleepTimer;
    }
    snapshot() {
        return {
            id: this.id,
            type: this.type,
            position: cloneVec3(this.position),
            rotation: cloneQuat(this.rotation),
            velocity: cloneVec3(this.velocity),
            angularVelocity: cloneVec3(this.angularVelocity),
            mass: this.mass,
            inverseMass: this.inverseMass,
            inverseInertia: cloneVec3(this.inverseInertia),
            sleeping: this.sleeping
        };
    }
}
function cloneQuat(value) {
    return [value[0], value[1], value[2], value[3]];
}
function normalizeQuat(value) {
    if (value.length !== 4 || !Number.isFinite(value[0]) || !Number.isFinite(value[1]) || !Number.isFinite(value[2]) || !Number.isFinite(value[3])) {
        throw new Error("body rotation must be a finite quaternion.");
    }
    const length = Math.hypot(value[0], value[1], value[2], value[3]);
    if (length <= 1e-9) {
        throw new Error("body rotation quaternion cannot be zero.");
    }
    return [value[0] / length, value[1] / length, value[2] / length, value[3] / length];
}
function inverseInertia(inertia) {
    validateFiniteVec3(inertia, "body inertia");
    if (inertia[0] <= 0 || inertia[1] <= 0 || inertia[2] <= 0) {
        throw new Error("dynamic body inertia must contain finite positive principal moments.");
    }
    return [1 / inertia[0], 1 / inertia[1], 1 / inertia[2]];
}
function multiplyVec3(a, b) {
    return [a[0] * b[0], a[1] * b[1], a[2] * b[2]];
}
function subVec3(a, b) {
    return [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
}
function crossVec3(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 integrateRotation(rotation, angularVelocity, dt) {
    const speed = Math.hypot(angularVelocity[0], angularVelocity[1], angularVelocity[2]);
    if (speed <= 1e-12) {
        return cloneQuat(rotation);
    }
    const halfAngle = speed * dt * 0.5;
    const sinHalf = Math.sin(halfAngle);
    const delta = [
        angularVelocity[0] / speed * sinHalf,
        angularVelocity[1] / speed * sinHalf,
        angularVelocity[2] / speed * sinHalf,
        Math.cos(halfAngle)
    ];
    return normalizeQuat(multiplyQuat(delta, rotation));
}
function multiplyQuat(a, b) {
    return [
        a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
        a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0],
        a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3],
        a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]
    ];
}
//# sourceMappingURL=RigidBody.js.map