broken Method to Sort by Z based on where the player is and is looking

[SpookyGhost]

I’ve got this FPS that I wanna get a problem sorted out on Z sorting. My Camera moves with my Player as per normal FPS games, but I need to Z sort my map items (billboarded Enemies, billboarded props, etc) and I just can’t figure it out.

Can anybody help me? The method is:

public static Vector<MapItem> sortByZ(Vector<MapItem> unsorted, Player player, boolean lastToFirst)

And I need it to work so that it’ll return a list of every MapItem in the unsorted Vector and sort them with the player’s position as the origin, and the player’s yaw as forward.

Note: This is Java using LWJGL.

Can someone please tell me what I’m doing wrong, and help me out?

Here are some files to help get as better grasp of whats going on:

import java.util.Vector;

import org.lwjgl.util.vector.Vector3f;

public class MapItem {
	protected Vector3f pos;
	
	protected float width;	
	protected float height;
	
	protected float distFromPlayer = Float.MAX_VALUE;
	
	//getters
		public Vector3f getPos(){
			return pos;
		}
		
		public float getX(){
			return pos.x;
		}
		
		public float getY(){
			return pos.y;
		}
		
		public float getZ(){
			return pos.z;
		}
		
		public float getWidth(){
			return width;
		}
		
		public float getHeight(){
			return height;
		}
		
		public float getDistFromPLayer(){
			return distFromPlayer;
		}
	
	//setters
		public void setPos(Vector3f npos){
			pos = npos;
		}
		
		public void setX(float x){
			pos.x = x;
		}
		
		public void setY(float y){
			pos.y = y;
		}
		
		public void setZ(float z){
			pos.z = z;
		}
		
		public void getWidth(float nwidth){
			width = nwidth;
		}
		
		public void getHeight(float nheight){
			height = nheight;
		}
		
		public void setDistFromPLayer(float ndist){
			distFromPlayer = ndist;
		}
		
	public static Vector<MapItem> sortByZ(Vector<MapItem> unsorted, Player player, boolean lastToFirst){
		Vector<MapItem> sorted = new Vector<MapItem>();
		
		for(MapItem mi : unsorted){
			mi.setDistFromPLayer((float) Math.abs(Math.sqrt(Math.pow(mi.getX() - player.getX(), 2) +  Math.pow(mi.getY() - player.getY(), 2) + Math.pow(-(mi.getZ() - player.getZ()), 2))));	
			
			
			if(sorted.size() <= 0) sorted.add(mi);
			
			else{
				boolean append = true;
				int indexToAdd = 0;
				for(int i = 0; i < sorted.size(); i++){
					MapItem smi = sorted.get(i);
					if(mi.getDistFromPLayer() < smi.getDistFromPLayer()){
						indexToAdd = i;
						append = false;
					}
				}
				
				if(append) sorted.insertElementAt(mi, sorted.size());
				else sorted.insertElementAt(mi, indexToAdd);
				
				
			}
		}
		
		if(lastToFirst){
			Vector<MapItem> rsorted = new Vector<MapItem>();
			for(int i = sorted.size() - 1; i >= 0; i--){
				rsorted.add(sorted.get(i));
			}
			
			sorted = rsorted;
		}
		
		return sorted;
	}
}

import org.lwjgl.util.vector.Vector3f;

public class Player {
	private Vector3f pos;
	private float yaw;
	
	private float walkSpeed = 1.0f;
	private float turnSpeed = 4.0f;
	
	public void walkForward()
	{
		Vector3f move = new Vector3f(0,0,0);
		move.x = (float)-Math.cos(Math.toRadians(yaw));
	    move.z = (float)-Math.sin(Math.toRadians(yaw));

	    Vector3f npos = new Vector3f(pos);
	    
	    move.normalise();
	    move.scale(movementSpeed); 
	    
	    Vector3f.add(pos, move, npos);
	}
	
	public void lookLeft(){
		yaw -= turnSpeed;
	}
	
	public void lookLeft(){
		yaw += turnSpeed;
	}
}