Renderer/src/main/java/electrosphere/game/collision/CollisionEngine.java

package electrosphere.game.collision;

import electrosphere.collision.dispatch.CollisionWorld.ClosestConvexResultCallback;
import electrosphere.collision.dispatch.CollisionWorld.LocalConvexResult;
import electrosphere.collision.broadphase.BroadphaseInterface;
import electrosphere.collision.broadphase.BroadphasePair;
import electrosphere.collision.broadphase.BroadphaseProxy;
import electrosphere.collision.broadphase.DbvtBroadphase;
import electrosphere.collision.broadphase.Dispatcher;
import electrosphere.collision.broadphase.OverlappingPairCache;
import electrosphere.collision.dispatch.CollisionDispatcher;
import electrosphere.collision.dispatch.CollisionObject;
import electrosphere.collision.dispatch.CollisionWorld.ClosestConvexResultCallback;
import electrosphere.collision.dispatch.CollisionWorld.LocalConvexResult;
import electrosphere.collision.dispatch.DefaultCollisionConfiguration;
import electrosphere.collision.narrowphase.ManifoldPoint;
import electrosphere.collision.narrowphase.PersistentManifold;
import electrosphere.collision.shapes.SphereShape;
import electrosphere.dynamics.DiscreteDynamicsWorld;
import electrosphere.dynamics.DynamicsWorld;
import electrosphere.dynamics.InternalTickCallback;
import electrosphere.dynamics.RigidBody;
import electrosphere.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
import electrosphere.util.ObjectArrayList;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.state.collidable.Impulse;
import electrosphere.entity.types.hitbox.HitboxData;
import electrosphere.game.collision.collidable.Collidable;
import static electrosphere.main.Main.deltaTime;
import java.util.ArrayList;
import java.util.List;
import org.joml.Vector3d;
import org.joml.Vector3f;

/**
 *
 * @author amaterasu
 */
public class CollisionEngine {
    
    DiscreteDynamicsWorld world;
    SequentialImpulseConstraintSolver solver;
    BroadphaseInterface broadphase;
    DefaultCollisionConfiguration collisionConfiguration;
    CollisionDispatcher dispatcher;
    InternalTickCallback callback;
    
    List<Entity> collisionEntities = new ArrayList();
    List<Entity> physicsEntities = new ArrayList();
    List<Entity> dynamicPhysicsEntities = new ArrayList();
    List<Entity> structurePhysicsEntities = new ArrayList();
    List<CollisionObject> collisionObject = new ArrayList();
    List<Collidable> collidableList = new ArrayList();
    
    static final float linearDamping = 0.02f;
    
    public CollisionEngine(){
        broadphase = new DbvtBroadphase();
        collisionConfiguration = new DefaultCollisionConfiguration();
        dispatcher = new CollisionDispatcher(collisionConfiguration);
        solver = new SequentialImpulseConstraintSolver();
        world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
        
//        CollisionShape boxShape = new BoxShape(new javax.vecmath.Vector3f(1,1,1));
//        float mass = 1.0f;
//        DefaultMotionState fallMotionState = new DefaultMotionState(new Transform(new javax.vecmath.Matrix4f(new javax.vecmath.Quat4f(0,0,0,1),new javax.vecmath.Vector3f(0,50,0),1.0f)));
//        RigidBodyConstructionInfo boxShapeRigidBodyCI = new RigidBodyConstructionInfo(mass, fallMotionState, boxShape);
//        RigidBody boxRigidBody = new RigidBody(boxShapeRigidBodyCI);
//        
//        world.addRigidBody(boxRigidBody);
        
        callback = new InternalTickCallback(){
            @Override
            public void internalTick(DynamicsWorld dw, float f) {
                Dispatcher dispatcher = dw.getDispatcher();
                int manifoldCount = dispatcher.getNumManifolds();
                for (int i = 0; i < manifoldCount; i++) {
                    PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i);
                    // The following two lines are optional.
                    CollisionObject object1 = (CollisionObject)manifold.getBody0();
                    CollisionObject object2 = (CollisionObject)manifold.getBody1();
                    Collidable physicsObject1 = (Collidable)object1.getUserPointer();
                    Collidable physicsObject2 = (Collidable)object2.getUserPointer();
                    boolean hit = false;
                    Vector3d normal = null;
                    float magnitude = 0.0f;
                    for (int j = 0; j < manifold.getNumContacts(); j++) {
                        ManifoldPoint contactPoint = manifold.getContactPoint(j);
                        if (contactPoint.getDistance() < 0.0f) {
                            magnitude = contactPoint.getDistance();
                            //linear dampen
                            magnitude = magnitude;// * (float)Math.pow(1.0f - linearDamping,deltaTime * 2);
                            hit = true;
//                            System.out.println(contactPoint.positionWorldOnA + " " + contactPoint.positionWorldOnB);
                            normal = new Vector3d(contactPoint.normalWorldOnB.x,contactPoint.normalWorldOnB.y,contactPoint.normalWorldOnB.z);
                            break;
                        }
                    }
                    if (hit) {
                        resolveCollision(physicsObject1,physicsObject2, normal, magnitude);
                        resolveCollision(physicsObject2,physicsObject1, new Vector3d(normal).mul(-1.0f), magnitude);
//                        System.out.println("HIT + " + normal);
                        // Collision happened between physicsObject1 and physicsObject2. Collision normal is in variable 'normal'.
                    }
                }
            }
        };
        world.setInternalTickCallback(callback, world);
        
        //https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=11507
        //https://www.google.com/search?client=firefox-b-1-d&q=bullet+set+position+and+check+if+collision
        //https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=11399
        
    }
    
    public static void resolveCollision(Collidable impactor, Collidable receiver, Vector3d normal, float magnitude){
        switch(receiver.getType()){
            case Collidable.TYPE_CREATURE:
                switch(impactor.getType()){
                    case Collidable.TYPE_TERRAIN:
//                        System.out.println(EntityUtils.getPosition(impactor.getParent()) + " " + EntityUtils.getPosition(receiver.getParent()));
//                        System.out.println();
//                        System.out.println("Terrain-creature collision: " + normal + " mag:" + magnitude);
                        receiver.addImpulse(new Impulse(new Vector3d(0,normal.y,0), -magnitude*2, Collidable.TYPE_TERRAIN));
                        break;
                    case Collidable.TYPE_CREATURE:
                        receiver.addImpulse(new Impulse(normal, magnitude, Collidable.TYPE_CREATURE));
                        break;
                    case Collidable.TYPE_STRUCTURE:
                        float realMag = 1f/(float)Math.pow(0.1, magnitude);
                        System.out.println(normal + " - " + realMag);
                        receiver.addImpulse(new Impulse(normal, realMag, Collidable.TYPE_STRUCTURE));
//                        System.out.println("Structure-creature collision");
                        break;
                }
                break;
        }
    }
    
    public void clearCollidableImpulseLists(){
        for(Collidable collidable : collidableList){
            collidable.clear();
        }
    }
    
    
    /**
     * 
     * @param e the entity that wants to move
     * @param positionToCheck the position that it wants to move to
     * @return true if it can occupy that position, false otherwise
     */
    public boolean checkCanOccupyPosition(CommonWorldData w, Entity e, Vector3d positionToCheck){
        boolean rVal = true;
        //
        // check world bounds
        //
        if(
                positionToCheck.x < w.getWorldBoundMin().x ||
                positionToCheck.z < w.getWorldBoundMin().z ||
                positionToCheck.x > w.getWorldBoundMax().x ||
                positionToCheck.z > w.getWorldBoundMax().z
                ){
            return false;
        }
//        //
//        // are we below the terrain?
//        //
//        if(w.getElevationAtPoint(positionToCheck) > positionToCheck.y){
//            return false;
//        }
        return rVal;
    }
    
    public void simulatePhysics(float time){
        world.performDiscreteCollisionDetection();
        callback.internalTick(world, time);
//        world.stepSimulation(time);
    }
    
    /**
     * 
     * @param e the entity that's trying to move
     * @param positionToCheck the position the entity wants to be at
     * @return the position the engine recommends it move to instead (this is
     * guaranteed to be a valid position)
     */
    public Vector3d suggestMovementPosition(CommonWorldData w, Entity e, Vector3d positionToCheck){
        Vector3d suggestedPosition = new Vector3d(positionToCheck);
        //
        // adjust for minimum height (Terrain)
        //
//        float heightMapBias = 0.00001f;
//        if(w.getElevationAtPoint(positionToCheck) > positionToCheck.y){
//            suggestedPosition.y = w.getElevationAtPoint(positionToCheck) + heightMapBias;
//        }
        //
        // adjust for world bounds
        //
        if(suggestedPosition.x < w.getWorldBoundMin().x){
            suggestedPosition.x = w.getWorldBoundMin().x;
        }
        if(suggestedPosition.z < w.getWorldBoundMin().z){
            suggestedPosition.z = w.getWorldBoundMin().z;
        }
        if(suggestedPosition.x > w.getWorldBoundMax().x){
            suggestedPosition.x = w.getWorldBoundMax().x;
        }
        if(suggestedPosition.z > w.getWorldBoundMax().z){
            suggestedPosition.z = w.getWorldBoundMax().z;
        }
        return suggestedPosition;
    }
    
    
    public void registerCollidableEntity(Entity collidable){
        collisionEntities.add(collidable);
    }
    
    public List<Entity> getCollisionEntities(){
        return collisionEntities;
    }
    
    
    
    public boolean collisionSphereCheck(Entity hitbox1, HitboxData hitbox1data, Entity hitbox2, HitboxData hitbox2data){
        Vector3d position1 = EntityUtils.getPosition(hitbox1);
        Vector3d position2 = EntityUtils.getPosition(hitbox2);
        float radius1 = hitbox1data.getRadius();
        float radius2 = hitbox2data.getRadius();
        double distance = position1.distance(position2);
        if(distance < radius1 + radius2){
            return true;
        } else {
            return false;
        }
    }
    
    public void registerPhysicsEntity(Entity physicsEntity){
        physicsEntities.add(physicsEntity);
    }
    
    public List<Entity> getPhysicsEntities(){
        return physicsEntities;
    }
    
    public void registerDynamicPhysicsEntity(Entity dynamicEntity){
        dynamicPhysicsEntities.add(dynamicEntity);
    }
    
    public List<Entity> getDynamicPhysicsEntities(){
        return dynamicPhysicsEntities;
    }
    
    public void registerStructurePhysicsEntity(Entity structureEntity){
        structurePhysicsEntities.add(structureEntity);
    }
    
    public List<Entity> getStructurePhysicsEntities(){
        return structurePhysicsEntities;
    }
    
    public void updateDynamicObjectTransforms(){
        for(Entity dynamicEntity : dynamicPhysicsEntities){
            CollisionObject rigidBody = (CollisionObject)dynamicEntity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY);
            Vector3f offset = (Vector3f)dynamicEntity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY_OFFSET);
            Vector3f newPosition = PhysicsUtils.getRigidBodyPosition(rigidBody).sub(offset);
//            System.out.println(rigidBody + " position " + newPosition);
//            System.out.println("Linear velocity: " + rigidBody.getLinearVelocity(new javax.vecmath.Vector3f()));
            EntityUtils.getPosition(dynamicEntity).set(newPosition);
        }
    }
    
    public void registerCollisionObject(CollisionObject object, Collidable collidable){
        world.addCollisionObject(object);
        object.setUserPointer(collidable);
        collidableList.add(collidable);
        collisionObject.add(object);
    }
    
    public void listBodyPositions(){
        for(CollisionObject body : collisionObject){
            System.out.println(body);
            System.out.println(PhysicsUtils.getRigidBodyPosition(body));
        }
    }
    
    /*
    Check if the entity is being accelerated by gravity
    */
    public boolean gravityCheck(CommonWorldData w, Entity e){
        double worldHeight = w.getElevationAtPoint(EntityUtils.getPosition(e));
        double entityHeight = EntityUtils.getPosition(e).y;
        return entityHeight > worldHeight + 0.1f;
    }
    
    public float sweepTest(CollisionObject object, Vector3f startPos, Vector3f endPos){
        SphereShape sphere = new SphereShape(0.1f);
//        CollisionObject collider = new CollisionObject();
//        collider.setCollisionShape(sphere);
        ClosestConvexResultCallbackImpl callback = new ClosestConvexResultCallbackImpl(startPos,endPos,object,dispatcher,world.getPairCache());
        callback.collisionFilterGroup = 1;
        callback.collisionFilterMask = 1;
        world.convexSweepTest(sphere, PhysicsUtils.jomlVecToTransform(startPos), PhysicsUtils.jomlVecToTransform(endPos), callback);
//        callback.hasHit()
        if(callback.hasHit()){
            return callback.closestHitFraction;
        } else {
            return -1.0f;
        }
    }

    
    
    
    
    
//    private static class SpecificConvexResultCallback extends ConvexResultCallback {
//        
//        @Override
//        public float addSingleResult(LocalConvexResult convexResult, boolean bln) {
//        }
//        
//    }
    
    
    private static class ClosestConvexResultCallbackImpl extends ClosestConvexResultCallback {
        
        CollisionObject me;
        private OverlappingPairCache pairCache;
        private Dispatcher dispatcher;
        
        public ClosestConvexResultCallbackImpl(Vector3f startPos, Vector3f endPos, CollisionObject me, Dispatcher dispatcher, OverlappingPairCache pairCache){
            super(PhysicsUtils.jomlToVecmathVector3f(startPos),PhysicsUtils.jomlToVecmathVector3f(endPos));
            this.me = me;
            this.pairCache = pairCache;
            this.dispatcher = dispatcher;
        }
        
        @Override
        public float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace) {
            if (convexResult.hitCollisionObject == me) {
                return 1f;
            }

            Vector3f linVelA = new Vector3f(), linVelB = new Vector3f();
            linVelA.sub(PhysicsUtils.vecmathToJomlVector3f(convexToWorld), PhysicsUtils.vecmathToJomlVector3f(convexFromWorld));
            linVelB.set(0f, 0f, 0f);//toB.getOrigin()-fromB.getOrigin();

            Vector3f relativeVelocity = new Vector3f();
            relativeVelocity.sub(linVelA, linVelB);
            // don't report time of impact for motion away from the contact normal (or causes minor penetration)
            if (convexResult.hitNormalLocal.dot(PhysicsUtils.jomlToVecmathVector3f(relativeVelocity)) >= -0f) {
                    return 1f;
            }

            return super.addSingleResult(convexResult, normalInWorldSpace);
        }
        
        @Override
        public boolean needsCollision(BroadphaseProxy proxy0) {
            // don't collide with itself
            if (proxy0.clientObject == me) {
                return false;
            }

            // don't do CCD when the collision filters are not matching
            if (!super.needsCollision(proxy0)) {
                return false;
            }

            CollisionObject otherObj = (CollisionObject)proxy0.clientObject;

            // call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
            if (dispatcher.needsResponse(me, otherObj)) {
                // don't do CCD when there are already contact points (touching contact/penetration)
                ObjectArrayList<PersistentManifold> manifoldArray = new ObjectArrayList<PersistentManifold>();
                BroadphasePair collisionPair = pairCache.findPair(me.getBroadphaseHandle(), proxy0);
                if (collisionPair != null) {
                    if (collisionPair.algorithm != null) {
                        //manifoldArray.resize(0);
                        collisionPair.algorithm.getAllContactManifolds(manifoldArray);
                        for (int j=0; j<manifoldArray.size(); j++) {
                            PersistentManifold manifold = manifoldArray.getQuick(j);
                            if (manifold.getNumContacts() > 0) {
                                return false;
                            }
                        }
                    }
                }
            }
            return true;
        }
    }
    
    public void deregisterPhysicsObject(CollisionObject object){
        if(collisionObject.contains(object)){
            collisionObject.remove(object);
        }
        world.removeCollisionObject(object);
    }
    
    public void deregisterRigidBody(RigidBody body){
        if(collisionObject.contains(body)){
            collisionObject.remove(body);
        }
        if((body) != null){
            body.destroy();
//            world.removeRigidBody(body);
        }
    }
    
    public void deregisterCollidableEntity(Entity e){
        if(collisionEntities.contains(e)){
            collisionEntities.remove(e);
        }
        if(physicsEntities.contains(e)){
            physicsEntities.remove(e);
        }
        if(dynamicPhysicsEntities.contains(e)){
            dynamicPhysicsEntities.remove(e);
        }
        if(structurePhysicsEntities.contains(e)){
            structurePhysicsEntities.remove(e);
        }
    }
    
}