// 
// Decompiled by Procyon v0.6.0
// 

package com.hypixel.hytale.server.npc.util;

import com.hypixel.hytale.server.npc.role.Role;
import com.hypixel.hytale.server.core.modules.entity.component.BoundingBox;
import com.hypixel.hytale.server.npc.entities.NPCEntity;
import com.hypixel.hytale.component.ComponentAccessor;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.component.Ref;
import com.hypixel.hytale.server.core.universe.world.chunk.WorldChunk;
import com.hypixel.hytale.math.util.ChunkUtil;
import com.hypixel.hytale.math.util.MathUtil;
import com.hypixel.hytale.server.core.universe.world.World;
import com.hypixel.hytale.math.shape.Box;
import com.hypixel.hytale.server.core.asset.type.blockhitbox.BlockBoundingBoxes;
import com.hypixel.hytale.protocol.BlockMaterial;
import com.hypixel.hytale.server.core.asset.type.blocktype.config.BlockType;
import com.hypixel.hytale.math.vector.Vector3f;
import com.hypixel.hytale.math.util.TrigMathUtil;
import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
import javax.annotation.Nonnull;
import com.hypixel.hytale.math.vector.Vector3d;

public class NPCPhysicsMath
{
    public static final double EPSILON_LENGTH = 1.0E-6;
    public static final double EPSILON_LENGTH_2 = 1.0E-12;
    
    private NPCPhysicsMath() {
    }
    
    public static boolean near(@Nonnull final Vector3d v, @Nonnull final Vector3d w) {
        return near(v, w, 1.0E-12);
    }
    
    public static boolean near(@Nonnull final Vector3d v, @Nonnull final Vector3d w, final double epsilonLength) {
        return v.distanceSquaredTo(w) <= epsilonLength;
    }
    
    public static boolean near(final double v, final double w) {
        return near(v, w, 1.0E-6);
    }
    
    public static boolean near(final double v, final double w, final double epsilonLength) {
        return Math.abs(v - w) <= epsilonLength;
    }
    
    public static float headingFromDirection(final double x, final double z, final float def) {
        final double s = x * x + z * z;
        return (s < 1.0E-12) ? def : PhysicsMath.headingFromDirection(x, z);
    }
    
    public static float pitchFromDirection(final double x, final double y, final double z, final float def) {
        final double s = x * x + z * z;
        return (s < 1.0E-12) ? def : TrigMathUtil.atan2(y, Math.sqrt(s));
    }
    
    @Nonnull
    public static Vector3d getViewDirection(@Nonnull final Vector3f lookDirection, @Nonnull final Vector3d outDirection) {
        return PhysicsMath.vectorFromAngles(lookDirection.getYaw(), lookDirection.getPitch(), outDirection);
    }
    
    public static double cosAngleBetweenVectors(@Nonnull final Vector3d v, @Nonnull final Vector3d w) {
        return cosAngleBetweenVectors(v, v.length(), w, w.length());
    }
    
    public static double cosAngleBetweenVectors(@Nonnull final Vector3d v, final double vLen, @Nonnull final Vector3d w, final double wLen) {
        return v.dot(w) / (vLen * wLen);
    }
    
    public static double cosAngleBetweenUnitVectors(@Nonnull final Vector3d v, @Nonnull final Vector3d w) {
        return v.dot(w);
    }
    
    public static void realignVector(@Nonnull final Vector3d v, @Nonnull final Vector3d w, final double cosine, @Nonnull final Vector3d result) {
        realignVector(v, v.length(), w, w.length(), cosine, result);
    }
    
    public static void realignVector(@Nonnull final Vector3d v, final double vLen, @Nonnull final Vector3d w, final double wLen, final double cosine, @Nonnull final Vector3d result) {
        final double sine = Math.sqrt(1.0 - cosine * cosine);
        final double mX = v.y * w.z - v.z * w.y;
        final double mY = v.z * w.x - v.x * w.z;
        final double mZ = v.x * w.y - v.y * w.x;
        final double nX = v.y * mZ - v.z * mY;
        final double nY = v.z * mX - v.x * mZ;
        final double nZ = v.x * mY - v.y * mX;
        final double nLen = Math.sqrt(dotProduct(nX, nY, nZ));
        final double c = cosine * wLen / vLen;
        final double d = sine * wLen / nLen;
        result.x = c * v.x - d * nX;
        result.y = c * v.y - d * nY;
        result.z = c * v.z - d * nZ;
    }
    
    public static void realignUnitVector(@Nonnull final Vector3d v, @Nonnull final Vector3d w, final double cosine, @Nonnull final Vector3d result) {
        final double sine = Math.sqrt(1.0 - cosine * cosine);
        final double mX = v.y * w.z - v.z * w.y;
        final double mY = v.z * w.x - v.x * w.z;
        final double mZ = v.x * w.y - v.y * w.x;
        final double nX = v.y * mZ - v.z * mY;
        final double nY = v.z * mX - v.x * mZ;
        final double nZ = v.x * mY - v.y * mX;
        final double nLen = Math.sqrt(dotProduct(nX, nY, nZ));
        final double d = sine / nLen;
        result.x = cosine * v.x - d * nX;
        result.y = cosine * v.y - d * nY;
        result.z = cosine * v.z - d * nZ;
        result.normalize();
    }
    
    public static boolean realignVectorReturnDirection(@Nonnull final Vector3d v, final double vLen, @Nonnull final Vector3d w, final double wLen, final double cosine, @Nonnull final Vector3d result) {
        final double sine = Math.sqrt(1.0 - cosine * cosine);
        final double mX = v.y * w.z - v.z * w.y;
        final double mY = v.z * w.x - v.x * w.z;
        final double mZ = v.x * w.y - v.y * w.x;
        final double nX = v.y * mZ - v.z * mY;
        final double nY = v.z * mX - v.x * mZ;
        final double nZ = v.x * mY - v.y * mX;
        final double nLen = Math.sqrt(dotProduct(nX, nY, nZ));
        final double c = cosine * wLen / vLen;
        final double d = sine * wLen / nLen;
        result.x = c * v.x - d * nX;
        result.y = c * v.y - d * nY;
        result.z = c * v.z - d * nZ;
        return dotProduct(v.x, v.y, v.z, nX, nY, nZ) > 0.0;
    }
    
    @Nonnull
    public static Vector3d createOrthogonalvector(@Nonnull final Vector3d in, @Nonnull final Vector3d out) {
        final double x = in.x;
        final double y = in.y;
        final double z = in.z;
        final double ax = Math.abs(x);
        final double ay = Math.abs(y);
        final double az = Math.abs(z);
        if (ax >= ay && ax >= az) {
            out.x = y;
            out.y = -x;
            out.z = 0.0;
        }
        else if (ay >= ax && ay >= az) {
            out.x = 0.0;
            out.y = z;
            out.z = -y;
        }
        else {
            out.x = -z;
            out.y = 0.0;
            out.z = x;
        }
        return out;
    }
    
    public static boolean inViewSector(final double xViewer, final double zViewer, final float heading, final float coneAngle, double xObject, double zObject) {
        if (coneAngle > 3.1415927f) {
            return !inViewSector(xViewer, zViewer, heading + 3.1415927f, 6.2831855f - coneAngle, xObject, zObject);
        }
        xObject -= xViewer;
        zObject -= zViewer;
        final double l = xObject * xObject + zObject * zObject;
        if (l <= 1.0E-6) {
            return true;
        }
        float angle;
        for (angle = PhysicsMath.headingFromDirection(xObject, zObject), angle -= heading; angle < -3.1415927f; angle += 6.2831855f) {}
        while (angle > 3.1415927f) {
            angle -= 6.2831855f;
        }
        angle *= 2.0f;
        return -coneAngle <= angle && angle <= coneAngle;
    }
    
    public static boolean isInViewCone(final double xViewer, final double yViewer, final double zViewer, final double xViewDirection, final double yViewDirection, final double zViewDirection, final float cosConeHalfAngle, final double xObject, final double yObject, final double zObject) {
        return isInViewCone(xViewDirection, yViewDirection, zViewDirection, cosConeHalfAngle, xObject - xViewer, yObject - yViewer, zObject - zViewer);
    }
    
    public static boolean isInViewCone(final double xViewDirection, final double yViewDirection, final double zViewDirection, final float cosConeHalfAngle, final double xObject, final double yObject, final double zObject) {
        if (cosConeHalfAngle >= 1.0f) {
            return true;
        }
        final double len = length(xObject, yObject, zObject);
        if (len < 1.0E-6) {
            return true;
        }
        final double dot = dotProduct(xViewDirection, yViewDirection, zViewDirection, xObject, yObject, zObject);
        return dot > cosConeHalfAngle * len;
    }
    
    public static boolean isInViewCone(@Nonnull final Vector3d viewer, @Nonnull final Vector3d viewDirection, final float cosConeHalfAngle, @Nonnull final Vector3d object) {
        return isInViewCone(viewer.x, viewer.y, viewer.z, viewDirection.x, viewDirection.y, viewDirection.z, cosConeHalfAngle, object.x, object.y, object.z);
    }
    
    public static boolean isInViewCone(@Nonnull final Vector3d viewer, @Nonnull final Vector3d viewDirection, final float cosConeHalfAngle, @Nonnull final Vector3d object, @Nonnull final Vector3d componentSelector) {
        final double cx = componentSelector.x;
        final double cy = componentSelector.y;
        final double cz = componentSelector.z;
        return isInViewCone(viewer.x * cx, viewer.y * cy, viewer.z * cz, viewDirection.x * cx, viewDirection.y * cy, viewDirection.z * cz, cosConeHalfAngle, object.x * cx, object.y * cy, object.z * cz);
    }
    
    public static float turnAngle(final float from, final float to) {
        float delta = PhysicsMath.normalizeAngle(to) - PhysicsMath.normalizeAngle(from);
        if (delta < -3.1415927f) {
            delta += 6.2831855f;
        }
        else if (delta > 3.1415927f) {
            delta -= 6.2831855f;
        }
        return delta;
    }
    
    public static float clampRotation(final float rotation, final float maxAngle) {
        if (rotation >= maxAngle) {
            return maxAngle;
        }
        if (rotation > -maxAngle) {
            return rotation;
        }
        return -maxAngle;
    }
    
    public static int intersectLineSphere(@Nonnull final Vector3d center, final double radius, @Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d x1, @Nonnull final Vector3d x2, final boolean segmentOnly) {
        x1.assign(q).subtract(p);
        x2.assign(p).subtract(center);
        final double a = x1.dot(x1);
        final double b = 2.0 * x1.dot(x2);
        final double c = x2.dot(x2) - radius * radius;
        double k = b * b - 4.0 * a * c;
        if (a == 0.0) {
            if (k != 0.0) {
                return 0;
            }
            x1.assign(p);
            return 1;
        }
        else {
            if (k < 0.0) {
                return 0;
            }
            if (k != 0.0) {
                k = Math.sqrt(k) / (2.0 * a);
                final double d = -b / (2.0 * a);
                double d2 = d - k;
                double d3 = d + k;
                if (d3 < d2) {
                    final double t = d2;
                    d2 = d3;
                    d3 = t;
                }
                if (segmentOnly) {
                    if (d2 > 1.0) {
                        return 0;
                    }
                    if (d2 >= 0.0) {
                        x1.assign(p).addScaled(x2, d2);
                        if (d3 <= 1.0) {
                            x2.scale(d3).add(p);
                            return 2;
                        }
                        return 1;
                    }
                    else if (d3 >= 0.0 && d3 <= 1.0) {
                        x1.assign(p).addScaled(x2, d2);
                        return 1;
                    }
                }
                x1.assign(p).addScaled(x2, d2);
                x2.scale(d3).add(p);
                return 2;
            }
            final double d = -b / (2.0 * a);
            if (segmentOnly && (d < 0.0 || d > 1.0)) {
                return 0;
            }
            x1.assign(p).addScaled(x2, d);
            return 1;
        }
    }
    
    public static double intersectLineSphereLerp(@Nonnull final Vector3d center, final double radius, @Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d t1, @Nonnull final Vector3d t2, @Nonnull final Vector3d componentSelector) {
        t1.assign(q).subtract(p).scale(componentSelector);
        t2.assign(p).subtract(center).scale(componentSelector);
        final double a = t1.dot(t1);
        final double b = 2.0 * t1.dot(t2);
        final double c = t2.dot(t2) - radius * radius;
        double k = b * b - 4.0 * a * c;
        if (a == 0.0) {
            return (k == 0.0) ? 0.0 : 1.0;
        }
        if (k < 0.0) {
            return 1.0;
        }
        if (k == 0.0) {
            final double d = -b / (2.0 * a);
            if (d < 0.0 || d > 1.0) {
                return 1.0;
            }
            return d;
        }
        else {
            k = Math.sqrt(k) / (2.0 * a);
            final double d = -b / (2.0 * a);
            double d2 = d - k;
            double d3 = d + k;
            if (d3 < d2) {
                final double t3 = d2;
                d2 = d3;
                d3 = t3;
            }
            if (d2 > 1.0) {
                return 1.0;
            }
            if (d2 >= 0.0) {
                return d2;
            }
            if (d3 >= 0.0 && d3 <= 1.0) {
                return d3;
            }
            return 1.0;
        }
    }
    
    public static double intersectLineSphereLerp(@Nonnull final Vector3d center, final double radius, @Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d componentSelector) {
        return intersectLineSphereLerp(center, radius, p, q, new Vector3d(), new Vector3d(), componentSelector);
    }
    
    public static double dotProduct(@Nonnull final Vector3d base, @Nonnull final Vector3d p, @Nonnull final Vector3d q) {
        final double dx = p.x - base.x;
        final double dy = p.y - base.y;
        final double dz = p.z - base.z;
        final double px = q.x - base.x;
        final double py = q.y - base.y;
        final double pz = q.z - base.z;
        return dx * px + dy * py + dz * pz;
    }
    
    public static double dotProduct(@Nonnull final Vector3d base, @Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d componentSelector) {
        final double dx = (p.x - base.x) * componentSelector.x;
        final double dy = (p.y - base.y) * componentSelector.y;
        final double dz = (p.z - base.z) * componentSelector.z;
        final double px = (q.x - base.x) * componentSelector.x;
        final double py = (q.y - base.y) * componentSelector.y;
        final double pz = (q.z - base.z) * componentSelector.z;
        return dx * px + dy * py + dz * pz;
    }
    
    public static double dotProduct(final double dx, final double dy, final double dz) {
        return dx * dx + dy * dy + dz * dz;
    }
    
    public static double dotProduct(final double px, final double py, final double pz, final double qx, final double qy, final double qz) {
        return px * qx + py * qy + pz * qz;
    }
    
    public static double dotProduct(final float dx, final float dy, final float dz) {
        return dx * dx + dy * dy + dz * dz;
    }
    
    public static double dotProduct(final float px, final float py, final float pz, final float qx, final float qy, final float qz) {
        return px * qx + py * qy + pz * qz;
    }
    
    private static double length(final double dx, final double dy, final double dz) {
        return Math.sqrt(dotProduct(dx, dy, dz));
    }
    
    public static void lerpDistance(@Nonnull final Vector3d start, @Nonnull final Vector3d end, final double distance, @Nonnull final Vector3d result) {
        lerp(start, end, distance / start.distanceTo(end), result);
    }
    
    public static void lerp(@Nonnull final Vector3d start, @Nonnull final Vector3d end, final double lambda, @Nonnull final Vector3d result) {
        final double dx = end.x - start.x;
        final double dy = end.y - start.y;
        final double dz = end.z - start.z;
        offsetVector(start, dx, dy, dz, lambda, result);
    }
    
    public static double lerp(final double a, final double b, final double s) {
        return (1.0 - s) * a + s * b;
    }
    
    public static void offsetVector(@Nonnull final Vector3d start, final double dx, final double dy, final double dz, final double lambda, @Nonnull final Vector3d result) {
        result.assign(start.x + lambda * dx, start.y + lambda * dy, start.z + lambda * dz);
    }
    
    public static void offsetVector(final double sx, final double sy, final double sz, final double dx, final double dy, final double dz, final double lambda, @Nonnull final Vector3d result) {
        result.assign(sx + lambda * dx, sy + lambda * dy, sz + lambda * dz);
    }
    
    public static void orthoComposition(@Nonnull final Vector3d start, @Nonnull final Vector3d end, @Nonnull final Vector3d ortho, final double distance, @Nonnull final Vector3d result) {
        final double dx = end.x - start.x;
        final double dy = end.y - start.y;
        final double dz = end.z - start.z;
        final double ox = dy * ortho.z - dz * ortho.y;
        final double oy = dz * ortho.x - dx * ortho.z;
        final double oz = dx * ortho.y - dy * ortho.x;
        offsetVector(end, ox, oy, oz, distance / length(ox, oy, oz), result);
    }
    
    public static void orthoComposition(@Nonnull final Vector3d start, @Nonnull final Vector3d end, final double distanceStart, @Nonnull final Vector3d ortho, final double distance, @Nonnull final Vector3d result) {
        final double dx = end.x - start.x;
        final double dy = end.y - start.y;
        final double dz = end.z - start.z;
        final double ox = dy * ortho.z - dz * ortho.y;
        final double oy = dz * ortho.x - dx * ortho.z;
        final double oz = dx * ortho.y - dy * ortho.x;
        final double lambda = distanceStart / length(dx, dy, dz);
        offsetVector(start.x + lambda * dx, start.y + lambda * dy, start.z + lambda * dz, ox, oy, oz, distance / length(ox, oy, oz), result);
    }
    
    public static float lookatHeading(@Nonnull final Vector3d self, @Nonnull final Vector3d pointOfInterest, final float headingHint) {
        final double dx = pointOfInterest.x - self.x;
        final double dz = pointOfInterest.z - self.z;
        if (dx == 0.0 && dz == 0.0) {
            return headingHint;
        }
        return PhysicsMath.headingFromDirection(dx, dz);
    }
    
    public static double blockEmptySpace(@Nonnull final BlockType blockType, final int rotation, @Nonnull final Direction direction) {
        if (blockType == null) {
            return 1.0;
        }
        if (blockType == BlockType.EMPTY || blockType.getMaterial() != BlockMaterial.Solid) {
            return 1.0;
        }
        final BlockBoundingBoxes blockBoundingBoxes = BlockBoundingBoxes.getAssetMap().getAsset(blockType.getHitboxTypeIndex());
        if (blockBoundingBoxes == null) {
            return 1.0;
        }
        final Box tempBoundingBox = blockBoundingBoxes.get(rotation).getBoundingBox();
        double d = 0.0;
        switch (direction.ordinal()) {
            case 0: {
                d = tempBoundingBox.min.x;
                break;
            }
            case 1: {
                d = 1.0 - tempBoundingBox.max.x;
                break;
            }
            case 2: {
                d = tempBoundingBox.min.y;
                break;
            }
            case 3: {
                d = 1.0 - tempBoundingBox.max.y;
                break;
            }
            case 4: {
                d = tempBoundingBox.min.z;
                break;
            }
            case 5: {
                d = 1.0 - tempBoundingBox.max.z;
                break;
            }
            default: {
                return 0.0;
            }
        }
        return (d > 0.0 && d < 1.0) ? d : 0.0;
    }
    
    public static double heightOverGround(@Nonnull final World world, final double x, final double y, final double z) {
        final int ix = MathUtil.floor(x);
        final int iy = MathUtil.floor(y);
        final int iz = MathUtil.floor(z);
        final WorldChunk chunk = world.getChunkIfInMemory(ChunkUtil.indexChunkFromBlock(ix, iz));
        if (chunk == null) {
            return iy + 1;
        }
        final BlockType blockType = chunk.getBlockType(ix, iy, iz);
        final int rotation = chunk.getRotationIndex(ix, iy, iz);
        return iy + blockHeight(blockType, rotation);
    }
    
    public static double heightOverGround(@Nonnull final World world, final double x, final double z) {
        final int ix = MathUtil.floor(x);
        final int iz = MathUtil.floor(z);
        final WorldChunk chunk = world.getChunkIfInMemory(ChunkUtil.indexChunkFromBlock(ix, iz));
        if (chunk == null) {
            return -1.0;
        }
        final int iy = chunk.getHeight(ix, iz);
        final BlockType blockType = chunk.getBlockType(ix, iy, iz);
        final int rotationIndex = chunk.getRotationIndex(ix, iy, iz);
        return iy + blockHeight(blockType, rotationIndex);
    }
    
    public static double blockHeight(@Nonnull final BlockType blockType, final int rotation) {
        return 1.0 - blockEmptySpace(blockType, rotation, Direction.NEG_Y);
    }
    
    public static double dotProduct(final double x, final double y, final double z, @Nonnull final Vector3d componentSelector) {
        return x * x * componentSelector.x + y * y * componentSelector.y + z * z * componentSelector.z;
    }
    
    public static double dotProduct(final double px, final double py, final double pz, final double qx, final double qy, final double qz, @Nonnull final Vector3d componentSelector) {
        return px * qx * componentSelector.x + py * qy * componentSelector.y + pz * qz * componentSelector.z;
    }
    
    public static double projectedLengthSquared(@Nonnull final Vector3d v, @Nonnull final Vector3d componentSelector) {
        return dotProduct(v.x, v.y, v.z, componentSelector);
    }
    
    public static double projectedLength(@Nonnull final Vector3d v, @Nonnull final Vector3d componentSelector) {
        return Math.sqrt(projectedLengthSquared(v, componentSelector));
    }
    
    public static int intersectSweptSpheres(@Nonnull final Vector3d p1, @Nonnull final Vector3d velocity1, @Nonnull final Vector3d p2, @Nonnull final Vector3d velocity2, final double radius, @Nonnull final Vector3d componentSelector, final double[] results) {
        return intersectSweptSpheres(p1.x, p1.y, p1.z, velocity1.x, velocity1.y, velocity1.z, p2.x, p2.y, p2.z, velocity2.x, velocity2.y, velocity2.z, radius, componentSelector, results);
    }
    
    public static int intersectSweptSpheresFootpoint(@Nonnull final Vector3d p1, @Nonnull final Vector3d velocity1, final double radius1, @Nonnull final Vector3d p2, @Nonnull final Vector3d velocity2, final double radius2, @Nonnull final Vector3d componentSelector, final double[] results) {
        return intersectSweptSpheres(p1.x, p1.y + radius1, p1.z, velocity1.x, velocity1.y, velocity1.z, p2.x, p2.y + radius2, p2.z, velocity2.x, velocity2.y, velocity2.z, radius1 + radius2, componentSelector, results);
    }
    
    public static int intersectSweptSpheres(final double p1x, final double p1y, final double p1z, final double velocity1x, final double velocity1y, final double velocity1z, final double p2x, final double p2y, final double p2z, final double velocity2x, final double velocity2y, final double velocity2z, final double radius, @Nonnull final Vector3d componentSelector, final double[] results) {
        final double px = (p2x - p1x) * componentSelector.x;
        final double py = (p2y - p1y) * componentSelector.y;
        final double pz = (p2z - p1z) * componentSelector.z;
        final double vx = (velocity2x - velocity1x) * componentSelector.x;
        final double vy = (velocity2y - velocity1y) * componentSelector.y;
        final double vz = (velocity2z - velocity1z) * componentSelector.z;
        final double a = dotProduct(vx, vy, vz);
        final double b = 2.0 * dotProduct(px, py, pz, vx, vy, vz);
        final double c = dotProduct(px, py, pz) - radius * radius;
        double k = b * b - 4.0 * a * c;
        if (k < 0.0) {
            return 0;
        }
        results[1] = (results[0] = -b / (2.0 * a));
        if (k == 0.0) {
            return 1;
        }
        k = Math.sqrt(k) / (2.0 * a);
        final int n = 0;
        results[n] -= k;
        final int n2 = 1;
        results[n2] += k;
        if (results[0] >= results[1]) {
            throw new IllegalArgumentException("IntersectSweptSpheres: Near result larger far result");
        }
        return 2;
    }
    
    public static double collisionSphereRadius(@Nonnull final Box boundingBox) {
        return collisionSphereRadius(boundingBox.max.x - boundingBox.min.x, boundingBox.max.z - boundingBox.min.z, boundingBox.max.z - boundingBox.min.z);
    }
    
    public static double collisionSphereRadius(final double boxWidth, final double boxDepth, final double boxHeight) {
        return Math.pow(boxWidth * boxHeight * boxDepth * 3.0 / 12.566370964050293, 0.3333333333333333);
    }
    
    public static double collisionSphereRadius(@Nonnull final Ref<EntityStore> ref, @Nonnull final ComponentAccessor<EntityStore> componentAccessor) {
        final NPCEntity npcComponent = componentAccessor.getComponent(ref, NPCEntity.getComponentType());
        if (npcComponent != null) {
            final Role role = npcComponent.getRole();
            final double radius = (role != null) ? role.getCollisionRadius() : -1.0;
            if (radius >= 0.0) {
                return radius;
            }
        }
        final BoundingBox boundingBoxComponent = componentAccessor.getComponent(ref, BoundingBox.getComponentType());
        final Box boundingBox = (boundingBoxComponent != null) ? boundingBoxComponent.getBoundingBox() : null;
        if (boundingBox != null) {
            return collisionSphereRadius(boundingBox.width(), boundingBox.depth(), boundingBox.height());
        }
        return 0.75;
    }
    
    public static double rayCircleIntersect(final double sx, final double sy, final double dx, final double dy, final double radius) {
        double a = dx * dx + dy * dy;
        final double b = 2.0 * (sx * dx + sy * dy);
        final double c = sx * sx + sy * sy - radius * radius;
        double k = b * b - 4.0 * a * c;
        if (k < 0.0) {
            return -1.0;
        }
        k = Math.sqrt(k);
        a *= 2.0;
        final double r1 = (-b - k) / a;
        final double r2 = (-b + k) / a;
        if (r1 < 0.0) {
            return (r2 < 0.0) ? -1.0 : r2;
        }
        if (r2 < 0.0) {
            return r1;
        }
        return (r1 < r2) ? r1 : r2;
    }
    
    public static double rayCircleIntersect(@Nonnull final Vector3d start, @Nonnull final Vector3d end, @Nonnull final Vector3d center, final double radius, @Nonnull final Vector3d normal) {
        if (normal.x == 0.0) {
            return rayCircleIntersect(start.y - center.y, start.z - center.z, end.y - start.y, end.z - start.z, radius);
        }
        if (normal.y == 0.0) {
            return rayCircleIntersect(start.x - center.x, start.z - center.z, end.x - start.x, end.z - start.z, radius);
        }
        return rayCircleIntersect(start.x - center.x, start.y - center.y, end.x - start.x, end.y - start.y, radius);
    }
    
    @Nonnull
    public static Vector3d projection(@Nonnull final Vector3d v, @Nonnull final Vector3d p, @Nonnull final Vector3d result) {
        result.assign(v).scale(p.dot(v) / v.dot(v));
        return result;
    }
    
    @Nonnull
    public static Vector3d rejection(@Nonnull final Vector3d v, @Nonnull final Vector3d p, @Nonnull final Vector3d result) {
        projection(v, p, result);
        result.negate().add(p);
        return result;
    }
    
    @Nonnull
    public static Vector3d subtractVector(@Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d result) {
        return result.assign(p.x - q.x, p.y - q.y, p.z - q.z);
    }
    
    @Nonnull
    public static Vector3d addDifference(@Nonnull final Vector3d result, @Nonnull final Vector3d p, @Nonnull final Vector3d q) {
        return result.add(p.x - q.x, p.y - q.y, p.z - q.z);
    }
    
    @Nonnull
    public static Vector3d projection(@Nonnull final Vector3d base, @Nonnull final Vector3d v, @Nonnull final Vector3d p, @Nonnull final Vector3d result) {
        subtractVector(v, base, result).scale(dotProduct(base, p, v) / dotProduct(base, v, v));
        return result;
    }
    
    @Nonnull
    public static Vector3d rejection(@Nonnull final Vector3d base, @Nonnull final Vector3d v, @Nonnull final Vector3d p, @Nonnull final Vector3d result) {
        projection(base, v, p, result).negate();
        addDifference(result, p, base);
        return result;
    }
    
    @Nonnull
    public static Vector3d multiply(@Nonnull final Vector3d v, @Nonnull final Vector3d w) {
        v.x *= w.x;
        v.y *= w.y;
        v.z *= w.z;
        return v;
    }
    
    public static double squaredDistProjected(final double px, final double py, final double pz, @Nonnull final Vector3d q, @Nonnull final Vector3d normal) {
        return squaredDistProjected(px, py, pz, q.x, q.y, q.z, normal);
    }
    
    public static double squaredDistProjected(final double px, final double py, final double pz, final double qx, final double qy, final double qz, @Nonnull final Vector3d normal) {
        double d2 = 0.0;
        if (normal.x == 0.0) {
            final double d3 = qx - px;
            d2 += d3 * d3;
        }
        if (normal.y == 0.0) {
            final double d3 = qy - py;
            d2 += d3 * d3;
        }
        if (normal.z == 0.0) {
            final double d3 = qz - pz;
            d2 += d3 * d3;
        }
        return d2;
    }
    
    public static double getProjectedDifference(@Nonnull final Vector3d p, @Nonnull final Vector3d q, @Nonnull final Vector3d componentSelector) {
        return (p.x - q.x) * (1.0 - componentSelector.x) + (p.y - q.y) * (1.0 - componentSelector.y) + (p.z - q.z) * (1.0 - componentSelector.z);
    }
    
    public static boolean isInvalid(final double v) {
        return Double.isNaN(v) || Double.isInfinite(v);
    }
    
    public static boolean isInvalid(@Nonnull final Vector3d v) {
        return isInvalid(v.x) || isInvalid(v.y) || isInvalid(v.z);
    }
    
    public static boolean isValid(final double v) {
        return Double.isFinite(v);
    }
    
    public static boolean isValid(@Nonnull final Vector3d v) {
        return isValid(v.x) && isValid(v.y) && isValid(v.z);
    }
    
    public static double jumpParameters(@Nonnull final Vector3d position, @Nonnull final Vector3d targetPosition, final double gravity, @Nonnull final Vector3d velocity) {
        final double dx = targetPosition.x - position.x;
        final double dz = targetPosition.z - position.z;
        final double x = Math.sqrt(dx * dx + dz * dz);
        final double y = targetPosition.y - position.y;
        final double d = Math.sqrt(x * x + y * y);
        final double s1 = y + d;
        final double s2 = y - d;
        final double v = Math.sqrt(s1 / gravity);
        final float phi = TrigMathUtil.atan(-v * v / (gravity * x));
        velocity.assign(dx, TrigMathUtil.sin(phi) * x, dz).setLength(v);
        return v;
    }
    
    public static double accelerate(double v, final double a, final double t, final double limitSpeed) {
        v += a * t;
        if (v > limitSpeed) {
            v = limitSpeed;
        }
        return v;
    }
    
    public static double deccelerateToStop(double v, final double a, final double t) {
        if (v < 0.0) {
            v += a * t;
            if (v > 0.0) {
                v = 0.0;
            }
        }
        else {
            v -= a * t;
            if (v < 0.0) {
                v = 0.0;
            }
        }
        return v;
    }
    
    @Nonnull
    public static Vector3d deccelerateToStop(@Nonnull final Vector3d v, final double a, final double t) {
        v.x = deccelerateToStop(v.x, a, t);
        v.y = deccelerateToStop(v.y, a, t);
        v.z = deccelerateToStop(v.z, a, t);
        return v;
    }
    
    public static double accelerateDrag(final double v, final double a, final double t, final double terminalVelocity, final double p) {
        return v + t * a * (1.0 - Math.pow(Math.abs(v / terminalVelocity), p));
    }
    
    public static double accelerateDragCapped(double v, final double a, final double t, final double terminalVelocity, final double p) {
        v = accelerateDrag(v, a, t, terminalVelocity, p);
        return (v <= terminalVelocity) ? v : terminalVelocity;
    }
    
    public static double accelerateDrag(final double v, final double a, final double t, final double terminalVelocity) {
        return accelerateDrag(v, a, t, terminalVelocity, 3.0);
    }
    
    public static double accelerateDragCapped(final double v, final double a, final double t, final double terminalVelocity) {
        return accelerateDragCapped(v, a, t, terminalVelocity, 3.0);
    }
    
    public static double accelerateToTargetSpeed(final double vCurrent, double vTarget, final double dt, final double accel, final double decel, final double vMin, final double vMax) {
        vTarget = MathUtil.clamp(vTarget, vMin, vMax);
        if (vCurrent == vTarget) {
            return vTarget;
        }
        double accelDrag;
        if (vCurrent == 0.0) {
            accelDrag = 0.0;
        }
        else if (vCurrent > 0.0) {
            accelDrag = -accel * Math.pow(Math.abs(vCurrent / vMax), 3.0);
        }
        else if (vMin < 0.0) {
            accelDrag = decel * Math.pow(Math.abs(vCurrent / vMin), 3.0);
        }
        else {
            accelDrag = accel * Math.pow(Math.abs(vCurrent / vMax), 3.0);
        }
        if (vCurrent < vTarget) {
            final double v = vCurrent + dt * (accelDrag + accel);
            return (v > vTarget) ? vTarget : v;
        }
        final double v = vCurrent + dt * (accelDrag - decel);
        return (v < vTarget) ? vTarget : v;
    }
    
    public static double accelerateToTargetSpeed(final double vCurrent, final double vTarget, final double dt, final double accel, final double decel, final double vMax) {
        return accelerateToTargetSpeed(vCurrent, vTarget, dt, accel, decel, 0.0, vMax);
    }
    
    public static double accelerateToTargetSpeed(final double vCurrent, final double vTarget, final double dt, final double accel, final double vMax) {
        return accelerateToTargetSpeed(vCurrent, vTarget, dt, accel, accel, 0.0, vMax);
    }
    
    public static double gravityDrag(final double v, final double a, final double t, final double terminalVelocity, final double p) {
        final double ratio = Math.abs(v / terminalVelocity);
        final double pow = Math.pow(ratio, p);
        final double dragAccel = a * pow;
        if (v < 0.0) {
            final double newV = v - t * (a - dragAccel);
            return (v < -terminalVelocity && newV > -terminalVelocity) ? (-terminalVelocity) : newV;
        }
        final double newV = v - t * (a + dragAccel);
        return (v > terminalVelocity && newV < terminalVelocity) ? terminalVelocity : newV;
    }
    
    public static double gravityDrag(final double v, final double a, final double t, final double terminalVelocity) {
        return gravityDrag(v, a, t, terminalVelocity, 3.0);
    }
    
    public enum Direction
    {
        POS_X, 
        NEG_X, 
        POS_Y, 
        NEG_Y, 
        POS_Z, 
        NEG_Z;
    }
}