import { Vector3 } from "../Maths/math.vector.js";
import { Plane } from "../Maths/math.plane.js";
const intersectBoxAASphere = (boxMin, boxMax, sphereCenter, sphereRadius) => {
    if (boxMin.x > sphereCenter.x + sphereRadius) {
        return false;
    }
    if (sphereCenter.x - sphereRadius > boxMax.x) {
        return false;
    }
    if (boxMin.y > sphereCenter.y + sphereRadius) {
        return false;
    }
    if (sphereCenter.y - sphereRadius > boxMax.y) {
        return false;
    }
    if (boxMin.z > sphereCenter.z + sphereRadius) {
        return false;
    }
    if (sphereCenter.z - sphereRadius > boxMax.z) {
        return false;
    }
    return true;
};
const getLowestRoot = (function () {
    const result = { root: 0, found: false };
    return function (a, b, c, maxR) {
        result.root = 0;
        result.found = false;
        const determinant = b * b - 4.0 * a * c;
        if (determinant < 0) {
            return result;
        }
        const sqrtD = Math.sqrt(determinant);
        let r1 = (-b - sqrtD) / (2.0 * a);
        let r2 = (-b + sqrtD) / (2.0 * a);
        if (r1 > r2) {
            const temp = r2;
            r2 = r1;
            r1 = temp;
        }
        if (r1 > 0 && r1 < maxR) {
            result.root = r1;
            result.found = true;
            return result;
        }
        if (r2 > 0 && r2 < maxR) {
            result.root = r2;
            result.found = true;
            return result;
        }
        return result;
    };
})();
/** @internal */
export class Collider {
    constructor() {
        // Implementation of the "Improved Collision detection and Response" algorithm proposed by Kasper Fauerby
        // https://www.peroxide.dk/papers/collision/collision.pdf
        this._collisionPoint = Vector3.Zero();
        this._planeIntersectionPoint = Vector3.Zero();
        this._tempVector = Vector3.Zero();
        this._tempVector2 = Vector3.Zero();
        this._tempVector3 = Vector3.Zero();
        this._tempVector4 = Vector3.Zero();
        this._edge = Vector3.Zero();
        this._baseToVertex = Vector3.Zero();
        this._destinationPoint = Vector3.Zero();
        this._slidePlaneNormal = Vector3.Zero();
        this._displacementVector = Vector3.Zero();
        /** @internal */
        this._radius = Vector3.One();
        /** @internal */
        this._retry = 0;
        /** @internal */
        this._basePointWorld = Vector3.Zero();
        this._velocityWorld = Vector3.Zero();
        this._normalizedVelocity = Vector3.Zero();
        this._collisionMask = -1;
    }
    get collisionMask() {
        return this._collisionMask;
    }
    set collisionMask(mask) {
        this._collisionMask = !isNaN(mask) ? mask : -1;
    }
    /**
     * Gets the plane normal used to compute the sliding response (in local space)
     */
    get slidePlaneNormal() {
        return this._slidePlaneNormal;
    }
    // Methods
    /**
     * @internal
     */
    _initialize(source, dir, e) {
        this._velocity = dir;
        this._velocitySquaredLength = this._velocity.lengthSquared();
        const len = Math.sqrt(this._velocitySquaredLength);
        if (len === 0 || len === 1.0) {
            this._normalizedVelocity.copyFromFloats(dir._x, dir._y, dir._z);
        }
        else {
            dir.scaleToRef(1.0 / len, this._normalizedVelocity);
        }
        this._basePoint = source;
        source.multiplyToRef(this._radius, this._basePointWorld);
        dir.multiplyToRef(this._radius, this._velocityWorld);
        this._velocityWorldLength = this._velocityWorld.length();
        this._epsilon = e;
        this.collisionFound = false;
    }
    /**
     * @internal
     */
    _checkPointInTriangle(point, pa, pb, pc, n) {
        pa.subtractToRef(point, this._tempVector);
        pb.subtractToRef(point, this._tempVector2);
        Vector3.CrossToRef(this._tempVector, this._tempVector2, this._tempVector4);
        let d = Vector3.Dot(this._tempVector4, n);
        if (d < 0) {
            return false;
        }
        pc.subtractToRef(point, this._tempVector3);
        Vector3.CrossToRef(this._tempVector2, this._tempVector3, this._tempVector4);
        d = Vector3.Dot(this._tempVector4, n);
        if (d < 0) {
            return false;
        }
        Vector3.CrossToRef(this._tempVector3, this._tempVector, this._tempVector4);
        d = Vector3.Dot(this._tempVector4, n);
        return d >= 0;
    }
    /**
     * @internal
     */
    _canDoCollision(sphereCenter, sphereRadius, vecMin, vecMax) {
        const distance = Vector3.Distance(this._basePointWorld, sphereCenter);
        const max = Math.max(this._radius.x, this._radius.y, this._radius.z);
        if (distance > this._velocityWorldLength + max + sphereRadius) {
            return false;
        }
        if (!intersectBoxAASphere(vecMin, vecMax, this._basePointWorld, this._velocityWorldLength + max)) {
            return false;
        }
        return true;
    }
    /**
     * @internal
     */
    _testTriangle(faceIndex, trianglePlaneArray, p1, p2, p3, hasMaterial, hostMesh) {
        let t0;
        let embeddedInPlane = false;
        //defensive programming, actually not needed.
        if (!trianglePlaneArray) {
            trianglePlaneArray = [];
        }
        if (!trianglePlaneArray[faceIndex]) {
            trianglePlaneArray[faceIndex] = new Plane(0, 0, 0, 0);
            trianglePlaneArray[faceIndex].copyFromPoints(p1, p2, p3);
        }
        const trianglePlane = trianglePlaneArray[faceIndex];
        if (!hasMaterial && !trianglePlane.isFrontFacingTo(this._normalizedVelocity, 0)) {
            return;
        }
        const signedDistToTrianglePlane = trianglePlane.signedDistanceTo(this._basePoint);
        const normalDotVelocity = Vector3.Dot(trianglePlane.normal, this._velocity);
        // if DoubleSidedCheck is false(default), a double sided face will be consided 2 times.
        // if true, it discard the faces having normal not facing velocity
        if (Collider.DoubleSidedCheck && normalDotVelocity > 0.0001) {
            return;
        }
        if (normalDotVelocity == 0) {
            if (Math.abs(signedDistToTrianglePlane) >= 1.0) {
                return;
            }
            embeddedInPlane = true;
            t0 = 0;
        }
        else {
            t0 = (-1.0 - signedDistToTrianglePlane) / normalDotVelocity;
            let t1 = (1.0 - signedDistToTrianglePlane) / normalDotVelocity;
            if (t0 > t1) {
                const temp = t1;
                t1 = t0;
                t0 = temp;
            }
            if (t0 > 1.0 || t1 < 0.0) {
                return;
            }
            if (t0 < 0) {
                t0 = 0;
            }
            if (t0 > 1.0) {
                t0 = 1.0;
            }
        }
        this._collisionPoint.copyFromFloats(0, 0, 0);
        let found = false;
        let t = 1.0;
        if (!embeddedInPlane) {
            this._basePoint.subtractToRef(trianglePlane.normal, this._planeIntersectionPoint);
            this._velocity.scaleToRef(t0, this._tempVector);
            this._planeIntersectionPoint.addInPlace(this._tempVector);
            if (this._checkPointInTriangle(this._planeIntersectionPoint, p1, p2, p3, trianglePlane.normal)) {
                found = true;
                t = t0;
                this._collisionPoint.copyFrom(this._planeIntersectionPoint);
            }
        }
        if (!found) {
            let a = this._velocitySquaredLength;
            this._basePoint.subtractToRef(p1, this._tempVector);
            let b = 2.0 * Vector3.Dot(this._velocity, this._tempVector);
            let c = this._tempVector.lengthSquared() - 1.0;
            let lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                t = lowestRoot.root;
                found = true;
                this._collisionPoint.copyFrom(p1);
            }
            this._basePoint.subtractToRef(p2, this._tempVector);
            b = 2.0 * Vector3.Dot(this._velocity, this._tempVector);
            c = this._tempVector.lengthSquared() - 1.0;
            lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                t = lowestRoot.root;
                found = true;
                this._collisionPoint.copyFrom(p2);
            }
            this._basePoint.subtractToRef(p3, this._tempVector);
            b = 2.0 * Vector3.Dot(this._velocity, this._tempVector);
            c = this._tempVector.lengthSquared() - 1.0;
            lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                t = lowestRoot.root;
                found = true;
                this._collisionPoint.copyFrom(p3);
            }
            p2.subtractToRef(p1, this._edge);
            p1.subtractToRef(this._basePoint, this._baseToVertex);
            let edgeSquaredLength = this._edge.lengthSquared();
            let edgeDotVelocity = Vector3.Dot(this._edge, this._velocity);
            let edgeDotBaseToVertex = Vector3.Dot(this._edge, this._baseToVertex);
            a = edgeSquaredLength * -this._velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
            b = 2 * (edgeSquaredLength * Vector3.Dot(this._velocity, this._baseToVertex) - edgeDotVelocity * edgeDotBaseToVertex);
            c = edgeSquaredLength * (1.0 - this._baseToVertex.lengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
            lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                const f = (edgeDotVelocity * lowestRoot.root - edgeDotBaseToVertex) / edgeSquaredLength;
                if (f >= 0.0 && f <= 1.0) {
                    t = lowestRoot.root;
                    found = true;
                    this._edge.scaleInPlace(f);
                    p1.addToRef(this._edge, this._collisionPoint);
                }
            }
            p3.subtractToRef(p2, this._edge);
            p2.subtractToRef(this._basePoint, this._baseToVertex);
            edgeSquaredLength = this._edge.lengthSquared();
            edgeDotVelocity = Vector3.Dot(this._edge, this._velocity);
            edgeDotBaseToVertex = Vector3.Dot(this._edge, this._baseToVertex);
            a = edgeSquaredLength * -this._velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
            b = 2 * (edgeSquaredLength * Vector3.Dot(this._velocity, this._baseToVertex) - edgeDotVelocity * edgeDotBaseToVertex);
            c = edgeSquaredLength * (1.0 - this._baseToVertex.lengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
            lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                const f = (edgeDotVelocity * lowestRoot.root - edgeDotBaseToVertex) / edgeSquaredLength;
                if (f >= 0.0 && f <= 1.0) {
                    t = lowestRoot.root;
                    found = true;
                    this._edge.scaleInPlace(f);
                    p2.addToRef(this._edge, this._collisionPoint);
                }
            }
            p1.subtractToRef(p3, this._edge);
            p3.subtractToRef(this._basePoint, this._baseToVertex);
            edgeSquaredLength = this._edge.lengthSquared();
            edgeDotVelocity = Vector3.Dot(this._edge, this._velocity);
            edgeDotBaseToVertex = Vector3.Dot(this._edge, this._baseToVertex);
            a = edgeSquaredLength * -this._velocitySquaredLength + edgeDotVelocity * edgeDotVelocity;
            b = 2 * (edgeSquaredLength * Vector3.Dot(this._velocity, this._baseToVertex) - edgeDotVelocity * edgeDotBaseToVertex);
            c = edgeSquaredLength * (1.0 - this._baseToVertex.lengthSquared()) + edgeDotBaseToVertex * edgeDotBaseToVertex;
            lowestRoot = getLowestRoot(a, b, c, t);
            if (lowestRoot.found) {
                const f = (edgeDotVelocity * lowestRoot.root - edgeDotBaseToVertex) / edgeSquaredLength;
                if (f >= 0.0 && f <= 1.0) {
                    t = lowestRoot.root;
                    found = true;
                    this._edge.scaleInPlace(f);
                    p3.addToRef(this._edge, this._collisionPoint);
                }
            }
        }
        if (found) {
            const distToCollisionSquared = t * t * this._velocitySquaredLength;
            if (!this.collisionFound || distToCollisionSquared < this._nearestDistanceSquared) {
                // if collisionResponse is false, collision is not found but the collidedMesh is set anyway.
                // onCollide observable are triggered if collideMesh is set
                // this allow trigger volumes to be created.
                if (hostMesh.collisionResponse) {
                    if (!this.intersectionPoint) {
                        this.intersectionPoint = this._collisionPoint.clone();
                    }
                    else {
                        this.intersectionPoint.copyFrom(this._collisionPoint);
                    }
                    this._nearestDistanceSquared = distToCollisionSquared;
                    this._nearestDistance = Math.sqrt(distToCollisionSquared);
                    this.collisionFound = true;
                }
                this.collidedMesh = hostMesh;
            }
        }
    }
    /**
     * @internal
     */
    _collide(trianglePlaneArray, pts, indices, indexStart, indexEnd, decal, hasMaterial, hostMesh, invertTriangles, triangleStrip = false) {
        if (triangleStrip) {
            if (!indices || indices.length === 0) {
                for (let i = 0; i < pts.length - 2; i += 1) {
                    const p1 = pts[i];
                    const p2 = pts[i + 1];
                    const p3 = pts[i + 2];
                    // stay defensive and don't check against undefined positions.
                    if (!p1 || !p2 || !p3) {
                        continue;
                    }
                    // Handles strip faces one on two is reversed
                    if ((invertTriangles ? 1 : 0) ^ i % 2) {
                        this._testTriangle(i, trianglePlaneArray, p1, p2, p3, hasMaterial, hostMesh);
                    }
                    else {
                        this._testTriangle(i, trianglePlaneArray, p2, p1, p3, hasMaterial, hostMesh);
                    }
                }
            }
            else {
                for (let i = indexStart; i < indexEnd - 2; i += 1) {
                    const indexA = indices[i];
                    const indexB = indices[i + 1];
                    const indexC = indices[i + 2];
                    if (indexC === 0xffffffff) {
                        i += 2;
                        continue;
                    }
                    const p1 = pts[indexA];
                    const p2 = pts[indexB];
                    const p3 = pts[indexC];
                    // stay defensive and don't check against undefined positions.
                    if (!p1 || !p2 || !p3) {
                        continue;
                    }
                    // Handles strip faces one on two is reversed
                    if ((invertTriangles ? 1 : 0) ^ i % 2) {
                        this._testTriangle(i, trianglePlaneArray, p1, p2, p3, hasMaterial, hostMesh);
                    }
                    else {
                        this._testTriangle(i, trianglePlaneArray, p2, p1, p3, hasMaterial, hostMesh);
                    }
                }
            }
        }
        else if (!indices || indices.length === 0) {
            for (let i = 0; i < pts.length; i += 3) {
                const p1 = pts[i];
                const p2 = pts[i + 1];
                const p3 = pts[i + 2];
                if (invertTriangles) {
                    this._testTriangle(i, trianglePlaneArray, p1, p2, p3, hasMaterial, hostMesh);
                }
                else {
                    this._testTriangle(i, trianglePlaneArray, p3, p2, p1, hasMaterial, hostMesh);
                }
            }
        }
        else {
            for (let i = indexStart; i < indexEnd; i += 3) {
                const p1 = pts[indices[i] - decal];
                const p2 = pts[indices[i + 1] - decal];
                const p3 = pts[indices[i + 2] - decal];
                if (invertTriangles) {
                    this._testTriangle(i, trianglePlaneArray, p1, p2, p3, hasMaterial, hostMesh);
                }
                else {
                    this._testTriangle(i, trianglePlaneArray, p3, p2, p1, hasMaterial, hostMesh);
                }
            }
        }
    }
    /**
     * @internal
     */
    _getResponse(pos, vel) {
        pos.addToRef(vel, this._destinationPoint);
        vel.scaleInPlace(this._nearestDistance / vel.length());
        this._basePoint.addToRef(vel, pos);
        pos.subtractToRef(this.intersectionPoint, this._slidePlaneNormal);
        this._slidePlaneNormal.normalize();
        this._slidePlaneNormal.scaleToRef(this._epsilon, this._displacementVector);
        pos.addInPlace(this._displacementVector);
        this.intersectionPoint.addInPlace(this._displacementVector);
        this._slidePlaneNormal.scaleInPlace(Plane.SignedDistanceToPlaneFromPositionAndNormal(this.intersectionPoint, this._slidePlaneNormal, this._destinationPoint));
        this._destinationPoint.subtractInPlace(this._slidePlaneNormal);
        this._destinationPoint.subtractToRef(this.intersectionPoint, vel);
    }
}
/**
 * If true, it check for double sided faces and only returns 1 collision instead of 2
 */
Collider.DoubleSidedCheck = false;
//# sourceMappingURL=collider.js.map