Collision manager for OpenGL in Java

CollisionManager.java

package ch.heigvd.infogr.maths.collision;

import ch.heigvd.infogr.maths.Vector3;
import ch.heigvd.infogr.maths.collision.algorithms.AaBbBoxedCollisionAlgorithm;
import ch.heigvd.infogr.maths.collision.algorithms.ICollisionAlgorithm;
import ch.heigvd.infogr.maths.collision.representation.IBoxedCollidable;
import ch.heigvd.infogr.maths.collision.representation.ICollidable;
import com.jogamp.opengl.GL2;
import javafx.util.Pair;

import java.util.HashMap;
import java.util.Map;

/**
 * Manages the algorithms of collisions and provides API to work with them.
 */
public class CollisionManager {

    /**
     * The set of algorithms, where the key is a pair of the types of the object presentation, that
     * inherits from {@link ICollidable}.
     *
     */
    private Map<Pair<Class<? extends ICollidable>, Class<? extends ICollidable>>, ICollisionAlgorithm> _algorithms;

    /**
     * Creates an instance of the class.
     */
    public CollisionManager() {
        _algorithms = new HashMap<>();

        initializeAlgorithms();
    }

    /**
     * Adds a given algorithm to a pair of types of {@link ICollidable} elements.
     */
    private <T extends ICollidable, U extends ICollidable> void addAlgorithm(Class<T> left, Class<U> right, ICollisionAlgorithm algorithm) {
        if(left.equals(right)) {
            _algorithms.put(new Pair<>(left, right), algorithm);
        } else {
            _algorithms.put(new Pair<>(left, right), algorithm);
            _algorithms.put(new Pair<>(right, left), algorithm);
        }
    }

    /**
     * Determines whether two {@link ICollidable} elements are colliding.
     */
    public boolean collide(ICollidable left, ICollidable right) {
        Pair<Class<? extends ICollidable>, Class<? extends ICollidable>> pair = new Pair<>(left.getClass(), right.getClass());

        ICollisionAlgorithm algorithm = _algorithms.getOrDefault(pair, tryAssignSubtypesForAlgorithm(left.getClass(), right.getClass()));

        if(algorithm != null) {
            return algorithm.collide(left, right);
        } else {
            throw new RuntimeException("The algorithm for the collision between the types" + pair.getKey().getTypeName() + " and " + pair.getValue().getTypeName() + " was not found.");
        }
    }

    /**
     * Initializes the available algorithms.
     */
    private void initializeAlgorithms() {
        addAlgorithm(IBoxedCollidable.class, IBoxedCollidable.class, new AaBbBoxedCollisionAlgorithm());
    }

    /**
     * Tries to find the corresponding algorithm based on collision inheritance.
     * If found, the pair of types is added to the map of algorithms.
     */
    private <T extends ICollidable, U extends ICollidable> ICollisionAlgorithm tryAssignSubtypesForAlgorithm(Class<T> left, Class<U> right) {
        for (Map.Entry<Pair<Class<? extends ICollidable>, Class<? extends ICollidable>>, ICollisionAlgorithm> types : _algorithms.entrySet()) {
            if(types.getKey().getKey().isAssignableFrom(left) && types.getKey().getValue().isAssignableFrom(right)) {
                _algorithms.put(new Pair<>(left, right), types.getValue());
                return types.getValue();
            }
        }

        return null;
    }

    /**
     * Dumps the OpenGL model view matrix.
     */
    private static double[] getTransformationMatrix(GL2 gl) {
        double[] rawMatrix = new double[16];

        gl.glGetDoublev(GL2.GL_MODELVIEW_MATRIX, rawMatrix, 0);

        return rawMatrix;
    }

    /**
     * Calculates the location of a point in the world.
     */
    public static Vector3 calculateSceneLocation(GL2 gl, double x, double y, double z) {
        double[] m = getTransformationMatrix(gl);

        double xp = m[0] * x + m[4] * y + m[8] * z + m[12];
        double yp = m[1] * x + m[5] * y + m[9] * z + m[13];
        double zp = m[2] * x + m[6] * y + m[10] * z + m[14];
        double wp = m[3] * x + m[7] * y + m[11] * z + m[15];

        xp /= wp;
        yp /= wp;
        zp /= wp;

        return new Vector3(xp, yp, zp);
    }
}