Syniobots detectAndResolve method

private void detectAndResolve(Entity dynamicEntity, CLocation locationD, CVelocity velocityD, CRigidBody rigidBodyD, Entity staticEntity) {
        // S = static entity
        // D = dynamic entity

        CLocation locationS = mapLocation.get(staticEntity);
        CRigidBody rigidBodyS = mapRigidBody.get(staticEntity);

        if (rigidBodyS.simplifiedBounds == null)
                return;

        if (AABB.testIntersection(rigidBodyD.simplifiedBounds, locationD.pos, rigidBodyS.simplifiedBounds, locationS.pos, mtVector)) {
                // resolve collision
                locationD.pos.add(mtVector);

                mtVector.normalizeSafe();
                mtVector.multiply(velocityD.vel.dot(mtVector));
                velocityD.vel.subtract(mtVector);
        }
}