package electrosphere.collision;

import java.util.LinkedList;
import java.util.List;

import org.joml.Matrix4d;
import org.joml.Quaterniond;
import org.joml.Vector3d;
import org.ode4j.ode.DBody;
import org.ode4j.ode.DGeom;

import electrosphere.collision.collidable.Collidable;
import electrosphere.engine.Globals;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityTags;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.ServerEntityUtils;
import electrosphere.entity.state.collidable.ClientCollidableTree;
import electrosphere.entity.state.collidable.ServerCollidableTree;
import electrosphere.entity.state.physicssync.ClientPhysicsSyncTree;
import electrosphere.entity.state.physicssync.ServerPhysicsSyncTree;
import electrosphere.entity.types.terrain.TerrainChunkData;
import electrosphere.game.data.collidable.CollidableTemplate;
import electrosphere.server.datacell.Realm;
import electrosphere.server.datacell.utils.ServerEntityTagUtils;

/**
 * Utilities for performing physics-related operations on entities (particularly attaching physics to them in the first place)
 */
public class PhysicsEntityUtils {

    /**
     * The linear velocity threshold to disable under
     */
    static final double LINEAR_THRESHOLD = 0.1;

    /**
     * The angular velocity threshold to disable under
     */
    static final double ANGULAR_THRESHOLD = 0.1;

    /**
     * The number of steps below threshold to disable after
     */
    static final int STEP_THRESHOLD = 4;

    /**
     * [CLIENT ONLY] Attaches a collidable template to a given entity
     * @param rVal The entity
     * @param physicsTemplate The collidable template
     */
    public static void clientAttachCollidableTemplate(Entity rVal, CollidableTemplate physicsTemplate){
        DBody rigidBody = null;
        Collidable collidable;
        double mass = 1.0f;
        if(physicsTemplate.getMass() != null){
            mass = physicsTemplate.getMass();
        }
        switch(physicsTemplate.getType()){
            case "CYLINDER": {

                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCylinderBody(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    physicsTemplate.getDimension1(),
                    physicsTemplate.getDimension2(),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setCylinderMass(
                        Globals.clientSceneWrapper.getCollisionEngine(),
                        rigidBody,
                        mass,
                        physicsTemplate.getDimension1(),
                        physicsTemplate.getDimension2(),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(Globals.clientSceneWrapper.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from center of entity position
                CollisionBodyCreation.setOffsetPosition(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and link to structures
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ClientCollidableTree tree = new ClientCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store values
                Matrix4d offsetTransform = new Matrix4d().translationRotate(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW() //rotate
                );
                if(physicsTemplate.isAngularlyStatic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                Globals.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CUBE": {
                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCubeBody(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setBoxMass(
                        Globals.clientSceneWrapper.getCollisionEngine(),
                        rigidBody,
                        mass,
                        new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(Globals.clientSceneWrapper.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from center of entity position
                CollisionBodyCreation.setOffsetPosition(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and link to structures
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ClientCollidableTree tree = new ClientCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store values
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                if(physicsTemplate.isAngularlyStatic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                Globals.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CAPSULE": {
                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCapsuleBody(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    physicsTemplate.getDimension2(),
                    physicsTemplate.getDimension2() - physicsTemplate.getDimension1() - physicsTemplate.getDimension1(),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setCapsuleMass(
                        Globals.clientSceneWrapper.getCollisionEngine(),
                        rigidBody,
                        mass,
                        physicsTemplate.getDimension1(),
                        physicsTemplate.getDimension2(),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(Globals.clientSceneWrapper.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from center of entity position
                CollisionBodyCreation.setOffsetPosition(
                    Globals.clientSceneWrapper.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and link to structures
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ClientCollidableTree tree = new ClientCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store values
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                if(physicsTemplate.isAngularlyStatic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.CLIENT_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                Globals.clientScene.registerEntityToTag(rVal, EntityTags.COLLIDABLE);
            } break;
        }
        //if we successfully attached the body, add a sync tree
        if(rigidBody != null){
            ClientPhysicsSyncTree.attachTree(rVal);
        }
    }


    /**
     * [SERVER ONLY] Attaches a collidable template to a given entity
     * @param realm The realm the entity is inside of
     * @param rVal The entity
     * @param physicsTemplate The collidable template
     */
    public static void serverAttachCollidableTemplate(Realm realm, Entity rVal, CollidableTemplate physicsTemplate){
        DBody rigidBody = null;
        Collidable collidable;
        double mass = 1.0f;
        if(physicsTemplate.getMass() != null){
            mass = physicsTemplate.getMass();
        }
        switch(physicsTemplate.getType()){
            case "CYLINDER": {

                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCylinderBody(
                    realm.getCollisionEngine(),
                    physicsTemplate.getDimension1(),
                    physicsTemplate.getDimension2(),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setCylinderMass(
                        realm.getCollisionEngine(),
                        rigidBody,
                        mass,
                        physicsTemplate.getDimension1(),
                        physicsTemplate.getDimension2(),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(realm.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from entity center
                CollisionBodyCreation.setOffsetPosition(
                    realm.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and attach tracking
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store data
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                if(physicsTemplate.isAngularlyStatic()){
                    realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CUBE": {
                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCubeBody(
                    realm.getCollisionEngine(),
                    new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setBoxMass(
                        realm.getCollisionEngine(),
                        rigidBody,
                        mass,
                        new Vector3d(physicsTemplate.getDimension1(),physicsTemplate.getDimension2(),physicsTemplate.getDimension3()),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(realm.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from entity center
                CollisionBodyCreation.setOffsetPosition(
                    realm.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and attach tracking
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store data
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                if(physicsTemplate.isAngularlyStatic()){
                    realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
            } break;
            case "CAPSULE": {
                //
                //create dbody
                rigidBody = CollisionBodyCreation.createCapsuleBody(
                    realm.getCollisionEngine(),
                    physicsTemplate.getDimension2(),
                    physicsTemplate.getDimension2() - physicsTemplate.getDimension1() - physicsTemplate.getDimension1(),
                    Collidable.TYPE_CREATURE_BIT
                );
                if(physicsTemplate.getMass() != null){
                    CollisionBodyCreation.setCapsuleMass(
                        realm.getCollisionEngine(),
                        rigidBody,
                        mass,
                        physicsTemplate.getDimension1(),
                        physicsTemplate.getDimension2(),
                        new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ()),
                        new Quaterniond(physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW())
                    );
                }
                CollisionBodyCreation.setAutoDisable(realm.getCollisionEngine(), rigidBody, true, LINEAR_THRESHOLD, ANGULAR_THRESHOLD, STEP_THRESHOLD);

                //
                //set offset from entity center
                CollisionBodyCreation.setOffsetPosition(
                    realm.getCollisionEngine(),
                    rigidBody,
                    new Vector3d(physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ())
                );

                //
                //create collidable and attach tracking
                collidable = new Collidable(rVal, Collidable.TYPE_CREATURE, true);
                if(physicsTemplate.getLinearFriction() != null){
                    collidable.getSurfaceParams().setLinearFriction(physicsTemplate.getLinearFriction());
                }
                if(physicsTemplate.getRollingFriction() != null){
                    collidable.getSurfaceParams().setRollingFriction(physicsTemplate.getRollingFriction());
                }
                ServerCollidableTree tree = new ServerCollidableTree(rVal,collidable,rigidBody);
                PhysicsEntityUtils.setDBody(rVal,rigidBody);

                //
                //store data
                Matrix4d offsetTransform = new Matrix4d().translationRotateScale(
                    physicsTemplate.getOffsetX(), physicsTemplate.getOffsetY(), physicsTemplate.getOffsetZ(), //translate
                    physicsTemplate.getRotX(), physicsTemplate.getRotY(), physicsTemplate.getRotZ(), physicsTemplate.getRotW(), //rotate
                    1, 1, 1 //scale
                );
                if(physicsTemplate.isAngularlyStatic()){
                    realm.getCollisionEngine().setAngularlyStatic(rigidBody, true);
                }
                if(physicsTemplate.getKinematic()){
                    Globals.clientSceneWrapper.getCollisionEngine().setKinematic(rigidBody);
                }
                rVal.putData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM, offsetTransform);
                rVal.putData(EntityDataStrings.PHYSICS_MODEL_TEMPLATE, physicsTemplate);
                rVal.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
                rVal.putData(EntityDataStrings.SERVER_COLLIDABLE_TREE, tree);

                rVal.putData(EntityDataStrings.PHYSICS_MASS, mass);

                realm.getCollisionEngine().registerCollisionObject(rigidBody, collidable);
                ServerEntityTagUtils.attachTagToEntity(rVal, EntityTags.COLLIDABLE);
            } break;
        }
        //if we successfully attached the body, add a sync tree
        if(rigidBody != null){
            ServerPhysicsSyncTree.attachTree(rVal);
        }
    }


    /**
     * [CLIENT ONLY] Given an entity and a terrain chunk description, create physics for the chunk and attach it to the entity
     * @param terrain The entity
     * @param data The terrain description
     * @return The rigid body created (note, attachment has already been performed)
     */
    public static void clientAttachTerrainChunkRigidBody(Entity terrain, TerrainChunkData data){
        DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(Globals.clientSceneWrapper.getCollisionEngine(), data, Collidable.TYPE_STATIC_BIT);
        Collidable collidable = new Collidable(terrain,Collidable.TYPE_TERRAIN, false);
        Globals.clientSceneWrapper.getCollisionEngine().registerCollisionObject(terrainBody, collidable);
        PhysicsEntityUtils.setDBody(terrain,terrainBody);
        terrain.putData(EntityDataStrings.PHYSICS_COLLIDABLE, collidable);
    }


    /**
     * [SERVER ONLY] Given an entity and a terrain chunk description, create physics for the chunk and attach it to the entity
     * @param terrain The entity
     * @param data The terrain description
     * @return The rigid body created (note, attachment has already been performed)
     */
    public static DBody serverAttachTerrainChunkRigidBody(Entity terrain, TerrainChunkData data){
        Realm terrainRealm = Globals.realmManager.getEntityRealm(terrain);
        DBody terrainBody = CollisionBodyCreation.generateBodyFromTerrainData(terrainRealm.getCollisionEngine(),data,Collidable.TYPE_STATIC_BIT);
        
        terrainRealm.getCollisionEngine().registerCollisionObject(terrainBody, new Collidable(terrain,Collidable.TYPE_TERRAIN, false));
        PhysicsEntityUtils.setDBody(terrain,terrainBody);
        
        return terrainBody;
    }

    /**
     * Repositions all active physics-scoped entities on a given realm
     * @param collisionEngine The realm's collision engine
     */
    public static void serverRepositionEntities(CollisionEngine collisionEngine){
        List<Entity> toReposition = new LinkedList<Entity>();
        List<Collidable> collidableList = collisionEngine.getCollidables();
        if(collidableList == null){
            collisionEngine.getCollidables();
            throw new Error("Collision engine collidables are null!");
        }
        for(Collidable collidable : collidableList){
            Entity entity = collidable.getParent();
            DBody body = PhysicsEntityUtils.getDBody(entity);
            if(body != null && body.isEnabled() && !body.isKinematic()){
                toReposition.add(entity);
            }
        }
        for(Entity parent : toReposition){
            ServerEntityUtils.repositionEntity(parent,EntityUtils.getPosition(parent));
        }
    }

    /**
     * Gets the transform from parent entity position to rigid body
     * @param entity The entity
     * @return The transform
     */
    public static Matrix4d getEntityCollidableTransform(Entity entity){
        return (Matrix4d) entity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY_TRANSFORM);
    }

    /**
     * Checks if the entity contains a dbody
     * @param entity the entity
     * @return true if contains, false otherwise
     */
    public static boolean containsDBody(Entity entity){
        return entity.containsKey(EntityDataStrings.PHYSICS_COLLISION_BODY);
    }

    /**
     * Sets the dbody on the entity
     * @param entity The entity
     * @param body The body
     */
    public static void setDBody(Entity entity, DBody body){
        if(body == null){
            throw new Error("Trying to set null DBody!");
        }
        entity.putData(EntityDataStrings.PHYSICS_COLLISION_BODY, body);
    }

    /**
     * Gets the DBody attached to an entity
     * @param entity the entity
     * @return The dbody if it exists, null otherwise
     */
    public static DBody getDBody(Entity entity){
        return (DBody)entity.getData(EntityDataStrings.PHYSICS_COLLISION_BODY);
    }

    /**
     * Sets the position of a DGeom
     * @param collisionEngine the collision engine
     * @param geom the geometry
     */
    public static void setGeometryPosition(CollisionEngine collisionEngine, DGeom geom, Vector3d position, Quaterniond rotation){
        collisionEngine.setGeomTransform(geom, position, rotation);
    }

    /**
     * Gets the collidable attached to the entity
     * @param entity The entity
     * @return The collidable if it exists, null otherwise
     */
    public static Collidable getCollidable(Entity entity){
        return (Collidable)entity.getData(EntityDataStrings.PHYSICS_COLLIDABLE);
    }
    
}