P3D confusion with translate() and collision

So I decided to try and experiment a bit with P3D. I researched about it, borrowed some code, wrote my own and came up with this thing:

import java.awt.MouseInfo;
import java.awt.Point;
import java.awt.Robot;
import java.awt.GraphicsEnvironment;
import java.util.HashMap;
import processing.core.*;
import processing.event.KeyEvent;

import queasycam.*;
QueasyCam cam;
Gun gun;

PMatrix3D baseMat;

PFont dosis;
PFont header;

PImage crosshair;

boolean jump = false;
boolean jump2 = false;
boolean shift = false;
boolean zoom = false;

float speed = 0.65;
float ypos = 10;

PGraphics pg;

//rendering settings
float fov = PI/3.0;
float cameraZ = (height/2.0) / tan(fov/2.0);
float nearClippingDistance = 0.01; // default is cameraZ/10.0

ArrayList<PVector> blocks = new ArrayList<PVector>();

void setup() {
  fullScreen(P3D);

  gun = new Gun();

  crosshair = loadImage("crosshair.png");
  
  dosis = createFont("Dosis-Medium",32);
  header = createFont("Norwester",32);
  
  l1set();

  //cam
  cam = new QueasyCam(this);
  cam.sensitivity = 0.25;
  cam.position.x = 40;

  baseMat = getMatrix(baseMat);

  noCursor();
}

void draw() {
  background(255);

  cam.speed = speed;
  perspective(fov, float(width)/float(height), nearClippingDistance, cameraZ*1000.0);

  jump();
  shift();

  l1();
  
  gun.display();

  debugHUD();
}

void keyPressed() {
  if (cam.position.y == 10 && key == 32 && shift == false) jump = true;
  if (key == CODED && keyCode == SHIFT && jump == false && jump2 == false) shift = true;
}

void keyReleased() {
  if (key == CODED && keyCode == SHIFT) shift = false;
}

void mouseReleased() {
}

//--------------------------------------------------------------------------------------------------

void cbox(int x, int z, int y, int xw, int zw, int yw) {
  pushMatrix();
  translate(x, y, z);
  fill(160);
  stroke(100);
  strokeWeight(1);
  translate(x, y, z);
  box(xw, yw, zw);//
  popMatrix();
  
  if (cam.position.x == x && cam.position.z >= z && cam.position.z <= z +zw && cam.position.y >=  y) cam.position.x = x - 1;
  if (cam.position.x == x + xw && cam.position.z >=  z && cam.position.z <= z +zw && cam.position.y >=  y) cam.position.x = x + xw + 1;
  
  if (cam.position.z == z && cam.position.x >=  x && cam.position.x <= x +xw && cam.position.y >=  y) cam.position.z = z - 1;
  if (cam.position.z == z + zw && cam.position.x >=  x && cam.position.x <= x +xw && cam.position.y >=  y) cam.position.z = z + zw + 1;
}

//--------------------------------------------------------------------------------------------------

void debugHUD() {
  //HUD
  cam.beginHUD();
  textFont(header);
  //debug info
  fill(0,140);
  rect(10,7,170,170);
  fill(255);
  textSize(25);
  text("Debug Info:", 20, 33);
  text(int(frameRate) + " FPS", 20, 60);
  text(blocks.size() + " Blocks", 20, 85);
  //real coordinate system
  text("playerX: " + int(cam.position.x), 20, 110);
  text("playerZ: " + int(cam.position.z), 20, 137);
  text("playerY: " + int(cam.position.y), 20, 165);
  //block by block coordinate system
  //text("playerX: " + int((cam.position.x + 5) / 10), 20, 110);
  //text("playerZ: " + int((cam.position.z + 5) / 10), 20, 137);
  //text("playerY: " + int(-((cam.position.y / 10)-1)), 20, 165);
  
  //crosshair
  //image(crosshair, width/2 - 35, height/2 -35, 70, 70);
  cam.endHUD();
}

//--------------------------------------------------------------------------------------------------

void l1set() {
  //setup boxes
  for (int x = 0; x < 300; x = x + 10) {
    for (int y = 0; y < 300; y = y + 10) {
      blocks.add(new PVector(x, 35, y));
    }
  }

  //for (int y = 25; y > -100; y = y - 10) {
  //  for (int z = 0; z < 300; z = z + 10) {
  //    blocks.add(new PVector(0, y, z));
  //  }
  //}

  //for (int y = 25; y > -100; y = y - 10) {
  //  for (int z = 0; z < 300; z = z + 10) {
  //    blocks.add(new PVector(300, y, z));
  //  }
  //}

  //for (int y = 25; y > -100; y = y - 10) {
  //  for (int z = 0; z < 300; z = z + 10) {
  //    blocks.add(new PVector(z, y, 0));
  //  }
  //}

  //for (int y = 25; y > -100; y = y - 10) {
  //  for (int z = 0; z < 300; z = z + 10) {
  //    blocks.add(new PVector(z, y, 300));
  //  }
  //}
}

void l1() {
  //map blocks
  pushMatrix();

  for (PVector p : blocks) {
    pushMatrix();
    translate(p.x, p.y, p.z);

    //fill & stroke
    fill(160);
    stroke(100);
    strokeWeight(1);

    box(10);

    popMatrix();
  }

  popMatrix();
  
  cbox(50,100,10,20,20,20); 
}

//--------------------------------------------------------------------------------------------------

void jump() {
  //jump
  if (jump == false && jump2 == false) {
    cam.position.y = ypos;
  }

  if (jump == true) {
    cam.position.y = cam.position.y - 4;
    if (cam.position.y < ypos + -30) {
      jump2 = true;
      jump = false;
    }
  }

  if (jump2 == true) {
    cam.position.y = cam.position.y + 3;
  }
  if (cam.position.y >= ypos) jump2 = false;
}

void shift() {
  //shift
  if (shift == true) {
    cam.position.y = cam.position.y + 8;
    speed = 0.30;
  } else speed = 0.65;
}

//--------------------------------------------------------------------------------------------------

class Gun {
  Gun() {
  }
  void display() {
    pushMatrix();
    translate(cam.position.x, cam.position.y, cam.position.z);
    rotateY(-cam.pan);
    rotateZ(cam.tilt);
    fill(60);
    stroke(0);
    translate(0.7, 0.3, 0.3);
    box(0.7, 0.15, 0.11);
    popMatrix();
  }
}

//--------------------------------------------------------------------------------------------------

//queasycam
public class QueasyCam {
  public final static String VERSION = "##library.prettyVersion##";

  public boolean controllable;
  public float speed;
  public float sensitivity;
  public PVector position;
  public float pan;
  public float tilt;
  public PVector velocity;
  public float friction;

  private PApplet applet;
  private Robot robot;
  private PVector center;
  private PVector up;
  private PVector right;
  private PVector forward;
  private PVector target;
  private Point mouse;
  private Point prevMouse;
  private HashMap<Character, Boolean> keys;

  public QueasyCam(PApplet applet) {
    this.applet = applet;
    applet.registerMethod("draw", this);
    applet.registerMethod("keyEvent", this);

    try {
      robot = new Robot();
    } 
    catch (Exception e) {
    }

    controllable = true;
    speed = 3f;
    sensitivity = 2f;
    position = new PVector(0f, 0f, 0f);
    up = new PVector(0f, 1f, 0f);
    right = new PVector(1f, 0f, 0f);
    forward = new PVector(0f, 0f, 1f);
    velocity = new PVector(0f, 0f, 0f);
    pan = 0f;
    tilt = 0f;
    friction = 0.75f;
    keys = new HashMap<Character, Boolean>();

    applet.perspective(PConstants.PI/3f, (float)applet.width/(float)applet.height, 0.01f, 1000f);
  }

  public QueasyCam(PApplet applet, float near, float far) {
    this.applet = applet;
    applet.registerMethod("draw", this);
    applet.registerMethod("keyEvent", this);

    try {
      robot = new Robot();
    } 
    catch (Exception e) {
    }

    controllable = true;
    speed = 3f;
    sensitivity = 2f;
    position = new PVector(0f, 0f, 0f);
    up = new PVector(0f, 1f, 0f);
    right = new PVector(1f, 0f, 0f);
    forward = new PVector(0f, 0f, 1f);
    velocity = new PVector(0f, 0f, 0f);
    pan = 0f;
    tilt = 0f;
    friction = 0.75f;
    keys = new HashMap<Character, Boolean>();

    applet.perspective(PConstants.PI/3f, (float)applet.width/(float)applet.height, near, far);
  }

  public void draw() {
    if (!controllable) return;

    mouse = MouseInfo.getPointerInfo().getLocation();
    if (prevMouse == null) prevMouse = new Point(mouse.x, mouse.y);

    int w = GraphicsEnvironment.getLocalGraphicsEnvironment().getMaximumWindowBounds().width;
    int h = GraphicsEnvironment.getLocalGraphicsEnvironment().getMaximumWindowBounds().height;

    if (mouse.x < 1 && (mouse.x - prevMouse.x) < 0) {
      robot.mouseMove(w-2, mouse.y);
      mouse.x = w-2;
      prevMouse.x = w-2;
    }

    if (mouse.x > w-2 && (mouse.x - prevMouse.x) > 0) {
      robot.mouseMove(2, mouse.y);
      mouse.x = 2;
      prevMouse.x = 2;
    }

    if (mouse.y < 1 && (mouse.y - prevMouse.y) < 0) {
      robot.mouseMove(mouse.x, h-2);
      mouse.y = h-2;
      prevMouse.y = h-2;
    }

    if (mouse.y > h-1 && (mouse.y - prevMouse.y) > 0) {
      robot.mouseMove(mouse.x, 2);
      mouse.y = 2;
      prevMouse.y = 2;
    }

    pan += PApplet.map(mouse.x - prevMouse.x, 0, applet.width, 0, PConstants.TWO_PI) * sensitivity;
    tilt += PApplet.map(mouse.y - prevMouse.y, 0, applet.height, 0, PConstants.PI) * sensitivity;
    tilt = clamp(tilt, -PConstants.PI/2.01f, PConstants.PI/2.01f);

    if (tilt == PConstants.PI/2) tilt += 0.001f;

    forward = new PVector(PApplet.cos(pan), PApplet.tan(tilt), PApplet.sin(pan));
    forward.normalize();
    right = new PVector(PApplet.cos(pan - PConstants.PI/2), 0, PApplet.sin(pan - PConstants.PI/2));

    target = PVector.add(position, forward);

    prevMouse = new Point(mouse.x, mouse.y);

    if (keys.containsKey('a') && keys.get('a')) velocity.add(PVector.mult(right, speed));
    if (keys.containsKey('d') && keys.get('d')) velocity.sub(PVector.mult(right, speed));
    if (keys.containsKey('w') && keys.get('w')) velocity.add(PVector.mult(forward, speed));
    if (keys.containsKey('s') && keys.get('s')) velocity.sub(PVector.mult(forward, speed));

    velocity.mult(friction);
    position.add(velocity);
    center = PVector.add(position, forward);
    applet.camera(position.x, position.y, position.z, center.x, center.y, center.z, up.x, up.y, up.z);
  }

  public void keyEvent(KeyEvent event) {
    char key = event.getKey();

    switch (event.getAction()) {
    case KeyEvent.PRESS: 
      keys.put(Character.toLowerCase(key), true);
      break;
    case KeyEvent.RELEASE:
      keys.put(Character.toLowerCase(key), false);
      break;
    }
  }

  public void beginHUD()
  {
    g.pushMatrix();
    g.hint(DISABLE_DEPTH_TEST);
    g.resetMatrix();
    g.applyMatrix(baseMat);
  }

  public void endHUD()
  {
    g.hint(ENABLE_DEPTH_TEST);
    g.popMatrix();
  }

  private float clamp(float x, float min, float max) {
    if (x > max) return max;
    if (x < min) return min;
    return x;
  }

  public PVector getForward() {
    return forward;
  }

  public PVector getUp() {
    return up;
  }

  public PVector getRight() {
    return right;
  }

  public PVector getTarget() {
    return target;
  }
}

Works pretty okay but first, about the gun class:
It works nicely if the cams not moving or moving sideways but when the cam starts moving forward or back (or jumps ) it behaves weirdly. Why does that happen?

translate(cam.position.x, cam.position.y, cam.position.z);

Isn’t what this translate statement does is basically makes the gun move with the player and set the rotation point of the gun as the player?
But it doesn’t move with the player if the player jumps and the rotation point changes if the player’s moving forward or backwards.

Second thing, about the cbox function I’m trying to make. I want it to be a simple box that has an x y z position and the player can collide with. But the xyz doesnt quite work. It is off the coordinates it is supposed to be.

translate(x, y, z);

Shouldn’t this translate statement work? What am I missing?
Actually I now realized, it is exactly 2 times the coordinates of where it should be.

Third thing: Also about cbox, I couldn’t quite figure out how to do a 4 way collision. I tried doing something a few times:

  if (cam.position.x == x && cam.position.z >= z && cam.position.z <= z +zw && cam.position.y >=  y) cam.position.x = x - 1;
  if (cam.position.x == x + xw && cam.position.z >=  z && cam.position.z <= z +zw && cam.position.y >=  y) cam.position.x = x + xw + 1;
  
  if (cam.position.z == z && cam.position.x >=  x && cam.position.x <= x +xw && cam.position.y >=  y) cam.position.z = z - 1;
  if (cam.position.z == z + zw && cam.position.x >=  x && cam.position.x <= x +xw && cam.position.y >=  y) cam.position.z = z + zw + 1;

This is the last one I tried but it’s not even close to working. Does anyone know a good way of doing this with queasycam? I also looked at the maze runner example that was included in queasy cam which has collision but I have no idea how it works.

Thanks!