Ray Casting в LWJGL 3 дает ответы на нечетные числа

Question

Так что я уже некоторое время работаю в LWJGL 3. Но пришло время делать кастинг лучей, я знаю, как отправить лучей. Но проблема заключается в проверке столкновения с ним, я использую свою камеру, чтобы получить вращение луча, и я использовал положение камеры в качестве источника луча. Но каждый раз, когда я пытаюсь проверить столкновение со сферой, это никогда не работает правильно. Текущий метод проверки того, пересекает ли луч сферу или нет, использует Maths.getDistanceRaySphere (), а не какой-либо из методов класса луча (пробовал те, которые также не работают).

Вот класс камеры:

public class Camera {
private Vector3f pos, rot, direction;
private float camspeed = 0.5f, rotspeed, camspeedY;
private double oldMouseX = 0, oldMouseY = 0, newMouseX, newMouseY;
private float x, z;

public Camera(Vector3f pos, Vector3f rot, float camspeed, float rotationspeed) {
    this.pos = pos;
    this.rot = rot;
    this.camspeed = camspeed;
    this.camspeedY = camspeed;
    this.rotspeed = rotationspeed;
}

public Camera(Vector3f pos, Vector3f rot) {
    this.pos = pos;
    this.rot = rot;
}


public void rotate(Vector3f amount) {
    rot.x += amount.x;
    rot.y += amount.y;
    rot.z += amount.z;
}

public void update(Input input) {

    newMouseX = input.getMousex();
    newMouseY = input.getMousey();

    float dx = (float) (newMouseX - oldMouseX);
    float dy = (float) (newMouseY - oldMouseY);
    rot = Maths.add(rot, new Vector3f(dy * rotspeed, dx * rotspeed, 0));
    //rot = Vector3f.add(rot, new Vector3f(dy * rotspeed, dx * rotspeed, 0), rot);
    FixCamRotation(rot);

    updatedir(rot, camspeed);


    if (input.keypressed(GLFW.GLFW_KEY_A)) pos = Vector3f.add(pos, new Vector3f(-direction.z, 0, direction.x), pos);
    if (input.keypressed(GLFW.GLFW_KEY_D)) pos = Vector3f.add(pos, new Vector3f(direction.z, 0, -direction.x), pos);
    if (input.keypressed(GLFW.GLFW_KEY_W)) pos = Vector3f.add(pos, new Vector3f(-direction.x, 0, -direction.z), pos);
    if (input.keypressed(GLFW.GLFW_KEY_S)) pos = Vector3f.add(pos, new Vector3f(direction.x, 0, direction.z), pos);
    if (input.keypressed(GLFW.GLFW_KEY_SPACE)) pos = Vector3f.add(pos, new Vector3f(0, camspeedY, 0), pos);
    if (input.keypressed(GLFW.GLFW_KEY_LEFT_SHIFT)) pos = Vector3f.add(pos, new Vector3f(0, -camspeedY, 0), pos);



    oldMouseX = newMouseX;
    oldMouseY = newMouseY;


}

private void updatedir(Vector3f rot, float camspeed) {
    direction = new Vector3f(GetdirX(rot, camspeed), GetdirY(rot, camspeed), GetdirZ(rot, camspeed));
}

private float GetdirX(Vector3f rotation, float camspeed) {
    return (float) Math.sin(Math.toRadians(-rotation.y)) * camspeed;
}

private float GetdirY(Vector3f rotation, float camspeed) {
    return (float) Math.sin(Math.toRadians(rotation.x)) * camspeed;
}

private float GetdirZ(Vector3f rotation, float camspeed) {
    return (float) Math.cos(Math.toRadians(-rotation.y)) * camspeed;
}

private void FixCamRotation(Vector3f rotation) {
    if(rotation.y > 360) {
        rotation.setY(0);
    }
    if(rotation.y < -360) {
        rotation.setY(0);
    }
    if(!(rot.x >= -90)) {
        rot.x = -90;
    }
    if(!(rot.x <= 90)) {
        rot.x = 90;
    }
}



public Vector3f getPos() {
    return pos;
}

public void setPos(Vector3f pos) {
    this.pos = pos;
}

public Vector3f getRot() {
    return rot;
}

public void setRot(Vector3f rot) {
    this.rot = rot;
}

public Matrix4f getViewmatrix() {
    return Maths.createViewMatrix(pos, rot);
}

public float getX() {
    return x;
}

public float getZ() {
    return z;
}

public float getCamSpeed() {
    return camspeed;
}

public Vector3f getDirection() {
    return direction;
}

}

Вот класс луча:

public class Ray {

private Vector3f slope, position;

public Ray(Vector3f origin, Vector3f slope) {
    this.slope = slope;
    this.position = new Vector3f(origin.x, origin.y, origin.z);
}


public void update(Camera cam) {
    position = new Vector3f(cam.getPos().x, cam.getPos().y, cam.getPos().z);
    slope = new Vector3f(cam.getDirection().getX(), cam.getDirection().getY(), cam.getDirection().getZ());
}

public static boolean checkCollisionObj(Ray ray, Vector3f pos, float radius, float times, float scale) {
    Vector3f tempslope = new Vector3f(ray.getDirection().x, ray.getDirection().y, ray.getDirection().z);
    Vector3f tempPos = new Vector3f(ray.position.x, ray.position.y, ray.position.z);
    boolean collides = false;
    for(float i = 1; i < times; i += scale) {
        Vector3f temp = new Vector3f(tempPos.x + (tempslope.x * i), tempPos.y + (tempslope.y * i), tempPos.z + (tempslope.z * i));
        float y = Maths.getDistanceFloat(temp, pos);
        if (y < radius) {
            collides = true;
            Debug.log(y);
        }
    }
    return collides;
}


public static boolean IntersectRaySphere(Ray ray, Vector3f sphereOrigin, float sphereRadius) {
    Vector3f m = Maths.sub(ray.getPosition(), sphereOrigin);
    float b = Vector3f.dot(m, ray.getDirection());
    float c = Vector3f.dot(m, m) - (sphereRadius * sphereRadius);
    //exit if r s origin outside s (c > 0) and r pointing away from s (b > 0)
    if((c > 0.0f) && (b > 0.0f)) return false;
    float discr = (b * b) - c;

    // A negative discriminant corresponds to ray missing sphere
    if (discr < 0.0f) return false;

    // Ray now found to intersect sphere, compute smallest t value of intersection
    float t = (float) (-b - Math.sqrt(discr));

    // If t is negative, ray started inside sphere so clamp t to zero
    if (t < 0.0f) t = 0.0f;
    return true;

}


public boolean checkCollisionObj(Vector3f pos, float radius, int times, float scale, Camera cam) {
    boolean collides = false;
    float y = 0;
    for(float i = 1; i < times; i += scale) {
        Vector3f temp = new Vector3f(position.x + (slope.x * i), position.y + (slope.y * i), position.z + (slope.z * i));
        y = Maths.getDistanceFloat(temp, pos);
        if (y < radius) {
            collides = true;
        }
    }
    return collides;
}


public Vector3f getDirection() {
    return slope;
}

public Vector3f getPosition() {
    return position;
}

public void setPosition(Vector3f position) {
    this.position = position;
}

}

Вот класс математики

public class Maths {




public static Matrix4f createprojectmatrix(float width, float height, float fov, float near, float far) {
    Matrix4f projectionMatrix = new Matrix4f();
    float fieldOfView = fov;
    float aspectRatio = (float)(width / height);
    float near_plane = near;
    float far_plane = far;

    float y_scale = (float) Math.tan(Math.toRadians(fieldOfView / 2f));
    float x_scale = y_scale / aspectRatio;
    float frustum_length = far_plane - near_plane;

    projectionMatrix.m00 = x_scale;
    projectionMatrix.m11 = y_scale;
    projectionMatrix.m22 = -((far_plane + near_plane) / frustum_length);
    projectionMatrix.m23 = -1;
    projectionMatrix.m32 = -((2 * near_plane * far_plane) / frustum_length);
    projectionMatrix.m33 = 0;

    return projectionMatrix;
}

public static Matrix4f createtransmatrix(Vector3f translation, Vector3f rotation, Vector3f scale) {
    Matrix4f transformation = new Matrix4f();
    transformation.setIdentity();
    Matrix4f.translate(translation, transformation, transformation);
    Matrix4f.rotate((float) Math.toRadians(rotation.x), new Vector3f(1, 0, 0), transformation, transformation);
    Matrix4f.rotate((float) Math.toRadians(rotation.y), new Vector3f(0, 1, 0), transformation, transformation);
    Matrix4f.rotate((float) Math.toRadians(rotation.z), new Vector3f(0, 0, 1), transformation, transformation);
    Matrix4f.scale(scale, transformation, transformation);
    return transformation;
}



 public static Matrix4f createViewMatrix(Vector3f position, Vector3f rotation) {
        Matrix4f viewMatrix = new Matrix4f();
        viewMatrix.setIdentity();
        Matrix4f.rotate((float) Math.toRadians(rotation.x), new Vector3f(1, 0, 0), viewMatrix,
                viewMatrix);
        Matrix4f.rotate((float) Math.toRadians(rotation.y), new Vector3f(0, 1, 0), viewMatrix, viewMatrix);
        Vector3f cameraPos = position;
        Vector3f negativeCameraPos = new Vector3f(-cameraPos.x,-cameraPos.y,-cameraPos.z);
        Matrix4f.translate(negativeCameraPos, viewMatrix, viewMatrix);
        return viewMatrix;
    }

    public static Vector3f getDistanceVec(Vector3f in, Vector3f in2) {
        float x = getDistance(in.x, in2.x);
        float y = getDistance(in.y, in2.y);
        float z = getDistance(in.z, in2.z);
        return new Vector3f(x, y, z);
    }

    public static float getDistanceFloat(Vector3f in, Vector3f in2) {
        float x = in.x - in2.x;
        float y = in.y - in2.y;
        float z = in.z - in2.z;
        float w = (float) Math.sqrt(sqr(x) + sqr(y) + sqr(z));
        return w;

    }

    public static float getDistanceFloat(Vector2f in, Vector3f in2) {
        float x = getDistance(in.x, in2.x);
        float y = getDistance(in.y, in2.y);
        float w = (float) Math.sqrt(x + y);
        return w;

    }

    public static float getDistanceRaySphere(Ray ray, Vector3f pos, float length) {
        float dist = 10000000;
        for (float i = 0; i < length; i += 0.05f) {
            Vector3f point_on_slope = Maths.sub(ray.getDirection(), i);
            Vector3f point = Maths.add(ray.getPosition(), point_on_slope);
            float tempdist = Maths.getDistanceFloat(pos, point);
            if (tempdist < dist) {
                dist = tempdist;
            }

        }
        return dist;
    }

    public static float sqr(float in) {
        return (in * in);
    }

    public static float getDistance(float in, float in2) {
        return (float) (Math.sqrt(Math.pow((in - in2), 2)));
    }

    public static Vector3f sub(Vector3f left, Vector3f right) {
        return new Vector3f(left.x - right.x, left.y - right.y, left.z - right.z);
    }

    public static Vector3f add(Vector3f left, Vector3f right) {
        return new Vector3f(left.x + right.x, left.y + right.y, left.z + right.z);
    }

    public static Vector3f mul(Vector3f left, Vector3f right) {
        return new Vector3f(left.x * right.x, left.y * right.y, left.z * right.z);
    }

    public static Vector3f div(Vector3f left, Vector3f right) {
        return new Vector3f(left.x / right.x, left.y / right.y, left.z / right.z);
    }

    public static Vector3f sub(Vector3f left, float right) {
        return new Vector3f(left.x - right, left.y - right, left.z - right);
    }

    public static Vector3f add(Vector3f left, float right) {
        return new Vector3f(left.x + right, left.y + right, left.z + right);
    }

    public static Vector3f mul(Vector3f left, float right) {
        return new Vector3f(left.x * right, left.y * right, left.z * right);
    }

    public static Vector3f div(Vector3f left, float right) {
        return new Vector3f(left.x / right, left.y / right, left.z / right);
    }

}

А вот основной класс

public class Main implements Runnable{

public Thread main;
public Window window;
public Vector4f backcolor = new Vector4f(0.9f, 0.7f, 0f, 1f);
public boolean cursor = false;
long lastTime = System.nanoTime();
final double amountOfTicks = 60.0;
double ns = 1000000000 / amountOfTicks;
double delta = 0;
int updates = 0;




public Material mat = new Material("PNG", "res/textures/I6JOM.png");
public Mesh mesh = OBJUtil.parseObjfile("res/models/stall.obj", mat);


public Shader shader = new Shader("/glsl/vertex.glsl", "/glsl/fragment.glsl");
public Renderer renderer = new Renderer(shader);

Image img = new Image();

public GameObject object = new GameObject(mesh, new Vector3f(0, 0, -1), new Vector3f(0, 0, 0), new Vector3f(1f, 1f, 1f));

public Camera cam = new Camera(new Vector3f(0, 0, -1), new Vector3f(90, 180, 0), 0.1f, 0.2f);

public Ray ray = new Ray(cam.getPos(), cam.getRot());

public void start() {
    Debug.log("start");
    main = new Thread(this, "game");
    main.start();
}

public void init() {
    Debug.log("init");
    window = new Window(1860, 960, "Untitled Game");
    window.setbackcolor(backcolor);
    window.create();
    mesh.create();
    shader.create();
    renderer.create(window);
}

public void close() {
    Debug.log("stop");
    window.destroy();
    mesh.destroy();
    shader.destroy();

}


public void run() {
    init();
    Debug.log("loop");
    while(!window.shouldclose() && !window.input.keypressed(GLFW.GLFW_KEY_ESCAPE)) {
        if(window.input.keypressed(GLFW.GLFW_KEY_F11)) window.togglefullscreen();
        if(window.input.mousepressed(GLFW.GLFW_MOUSE_BUTTON_RIGHT)) window.setMouseState(!cursor);
        update();
        render();
    }
    close();
}

public void update() {
    float scale = 100;
    window.update(cam.getPos(), Maths.getDistanceRaySphere(ray, object.getPos(), scale));
    long now = System.nanoTime();
    delta += (now - lastTime) / ns;
    lastTime = now;
    if (delta >= 1) {
        tick();
        updates++;
        delta--;
    }
}

public void tick() {
    cam.update(window.input);
    ray.update(cam);
}

public void render() {
    renderer.render(object, cam);
    window.swapbuffers();
}

public static void main(String[] args) {
    new Main().start();
}

}