Get entities given player is looking at

gistfile1.java

	private static List<LivingEntity> getEntitiesLookingAt(Location p, Player player, double total, Timestamp.TimestampType type) {

		double yaw = p.getYaw() > 0 ? p.getYaw() : 360 - Math.abs(p.getYaw()); // remove negative degrees
		yaw += 90; // rotate +90 degrees
		if (yaw > 360)
			yaw -= 360;
		yaw  = (yaw  * Math.PI) / 180;
		double pitch  = ((p.getPitch() + 90)  * Math.PI) / 180;

		ArrayList<LivingEntity> entities = new ArrayList<>();

		// get entities in render area
		for (LivingEntity entity : BuildingUtils.getEntitiesInRadius(p, 200, player)) {

			// bounding box of select entity
			AxisAlignedBB box = ((CraftLivingEntity) entity).getHandle().boundingBox;

			// two dimensional coordinates that correspond to the corners of the bounding box on the horizontal plane (x, z)
			double[][] hCoords = new double[][] {
					{box.a, box.c}, {box.d, box.c}, {box.a, box.f}, {box.d, box.f}
			};

			Double[] thetas = new Double[4];

			for (int t = 0; t < 4; t++) {
				// x location of the corner relative to the origin (p)
				double xo = hCoords[t][0] - p.getX();
				// z location
				double zo = hCoords[t][1] - p.getZ();
				// get angle!
				double theta = toPolar(xo, zo);
				thetas[t] = theta;
			}
			// Calculate the required range for this entity
			Range<Double, Double> yawRange = getLargestArcRange(thetas);

			// Doing this for pitch is a bit more difficult, we need to use all corners of the bounding box in 3D space
			double[][] corners = new double[][] {
					{box.a, box.b, box.c}, {box.d, box.b, box.c}, {box.a, box.b, box.f}, {box.d, box.b, box.f},
					{box.a, box.e, box.c}, {box.d, box.e, box.c}, {box.a, box.e, box.f}, {box.d, box.e, box.f},
			};
			Double[] phis = new Double[8];
			for (int t = 0; t < 8; t++) {
				// x location of the corner relative to the origin (p)
				double xo = corners[t][0] - p.getX();
				// y location
				double yo = corners[t][1] - p.getY();
				// z location
				double zo = corners[t][2] - p.getZ();
				// convert to phi angle in spherical coordinates
				phis[t] = Math.acos(yo / Math.sqrt((xo * xo) + (yo * yo) + (zo * zo)));
			}

			Range<Double, Double> pitchRange = getLargestArcRange(phis);

			if (inside(yaw, yawRange.getLowest(), yawRange.getHighest()) && inside(pitch, pitchRange.getLowest(), pitchRange.getHighest()))
				entities.add(entity);
		}

		return entities;
	}
	// Sorts combinations of angles and returns the combination of angles with the largest distance between them
	public static Range<Double, Double> getLargestArcRange(Double[] angles) {
		Set<Set<Double>> combinations = getCombinationsFor(Arrays.asList(angles), 2);
		Range<Double, Double> largestRange = null;
		Double largest = null;
		for (Set<Double> combo : combinations) {
			Double[] array = combo.toArray(new Double[2]);
			double arc = distance(array[0], array[1]);

			if (largest == null || arc > largest) {
				largest = arc;
				largestRange = new Range<>(array[0], array[1]);
			}
		}
		if (largestRange != null && largestRange.getLowest() > largestRange.getHighest())
			largestRange = new Range<>(largestRange.getHighest(), largestRange.getLowest());
		return largestRange;
	}
	// Found from stackoverflow somewhere, because I'm lazy. I rewrote it to use generics, it's useful for sorting.
	public static <T> Set<Set<T>> getCombinationsFor(List<T> group, int k) {

		Set<Set<T>> allCombos = new HashSet<>();
		if (k == 0) {
			allCombos.add(new HashSet<T>());
			return allCombos;
		}
		if (k > group.size())
			return allCombos;

		List<T> groupWithoutX = new ArrayList<>(group);
		T x = groupWithoutX.remove(groupWithoutX.size()-1);

		Set<Set<T>> combosWithoutX = getCombinationsFor(groupWithoutX, k);
		Set<Set<T>> combosWithX = getCombinationsFor(groupWithoutX, k-1);
		for (Set<T> combo : combosWithX)
			combo.add(x);
		allCombos.addAll(combosWithoutX);
		allCombos.addAll(combosWithX);
		return allCombos;
	}

	// distance between two angles, in radians
	private static double distance(double r1, double r2) {
		double d = Math.abs(r2 - r1);
		if (d <= Math.PI)
			return d;
		else return 2 * Math.PI - d;
	}

	// [0, 2pi)
	private static double toPolar(double x, double y) {
		double theta = Math.atan2(y, x);
		if (theta < 0)
			return 2 * Math.PI + theta;
		else return theta;
	}

	private static boolean inside(double theta, double small, double large) {
		// If the range includes 0 rad
		if (large - small > Math.PI) {
			return theta > large || theta < small;
		}
		else {
			return theta < large && theta > small;
		}
	}

	private static class Range<L extends Number, H extends Number> {
		L lowest;
		H highest;
		Range(L lowest, H highest) {
			this.lowest = lowest;
			this.highest = highest;
		}
		public L getLowest() {
			return lowest;
		}
		public H getHighest() {
			return highest;
		}
		public void setKey(L lowest) {
			this.lowest = lowest;
		}
		public void setValue(H highest) {
			this.highest = highest;
		}
	}