findBlockPossesInSphericalCone.java

Set<BlockPos> findBlockPossesInSphericalCone(Player player, int coneAngle, int reach) {
	Vec3D playerHeadPosition = player.getHeadPosition();
	Vec3D originPosition = playerHeadPosition.transform( -0.5, -0.5, -0.5); //shift position to check for middlepoints of blocks.
	Vec3D lookVector = player.getLookVector; //I assume this will return a Vector with three factors <= 1.0
	Vec3D targetPosition = originPosition.transform(lookVector * reach);
	
	int coneX = lookVector.X;
	int coneY = lookVector.Y;
	int coneZ = lookVector.Z;
	
	//minimise `squareroot(square(coneX * x - blockX) + square(coneY * x - blockY) + square(coneZ * x - blockZ))` by changing x, which is the only variable the rest already has a value by now
	//using a and b from the abc method.
	int a = square(coneX) + (square(coneY) + square(coneZ); //the sum of three squares is always positive, so this formula has a minimum
	
	Set<BlockPos> investigatedPositions = new HashSet<BlockPos>(); //contains all positions which have been evaluated whether they are in the "cone" or not
	Set<BlockPos> validPositions = new HashSet<BlockPos>(); //contains all positions found to be within the "cone"
	List<BlockPos> unExploredPositions = new ArrayList<BlockPos>(); //contains valid positions of which the neighbours have not been investigated yet.
	
	for (double i = 0; i < reach; i += 0.5) { //walk along the cone's guideline from the "bottom" of the cone to the "top" of the cone, a.k.a. the @code{originPosition}
		BlockPos startingPos = originPosition.transform(lookVector * reach - i).findNearestBlockPos(); 
		
		if (investigatedPositions.contains(startingPos) {
			//do nothing
		} else {
			investigatedPositions.add(startingPos);
			if (isPosInSphere(startingPos, originPosition, reach) && isPosInCone(startingPos, originPosition, coneX, coneY, coneZ, a, coneAngle/2)) {
				validPositions.add(startingPos);
				unExploredPositions.add(startingPos);
			}
		}
		
		while (!unExploredPositions.empty) {
			BlockPos oldPosition = unExploredPositions.get(0);
			unExploredPositions.remove(position);
			
			//explore every position directly around
			for (int offsetX = -1; offsetX <= 1; offsetX++) {
				for (int offsetY = -1; offsetY <= 1; offsetY++) {
					for (int offsetZ = -1; offsetZ <= 1; offsetZ++) {
						if (offsetX == 0 && offsetY == 0 && offsetZ == 0) {
							//do nothing
						} else {
							BlockPos newPosition = oldPosition.offset(offsetX, offsetY, offsetZ);
							if (investigatedPositions.contains(newPosition) { //todo investigatedPositions can become pretty large, slowing down lookup time. Can this be remedied? A List type that will always be searched from back to front on contains(), perhaps?
								//do nothing
							} else {
								investigatedPositions.add(newPosition);
								if (isPosInSphere(newPosition, originPosition, reach) && isPosInCone(newPosition, originPosition, coneX, coneY, coneZ, a, coneAngle/2)) {
									validPositions.add(newPosition);
									unExploredPositions.add(newPosition);
								}
							}
						}
					}
				}
			}
		}
		continue; //:P
	}
	return validPositions;
}
		
boolean isPosInSphere(BlockPos pos, Vec3D middlePoint, int radius) {
	return squareRoot( square(xDiff) + square(yDiff) + square(zDiff) ) < radius
}

//coneVector should be the direction of the look Vector, not the absolute position
boolean isPosInCone(BlockPos pos, Vec3D topPoint, coneX, coneY, coneZ, a, int slopeAngle) {
	
	Vec3D blockVector = pos - topPoint; //transform BlockPos to its relative position
	int blockX = blockVector.x;
	int blockY = blockVector.y;
	int blockZ = blockVector.z;
	
	//minimise `squareroot(square(coneX * x - blockX) + square(coneY * x - blockY) + square(coneZ * x - blockZ))` by changing x, which is the only variable the rest already has a value by now
	//using a and b from the abc method. a comes as a parameter
	int b = -2 * (blockX * coneX + blockY * coneY + blockZ * coneZ);
	int c = square(blockX) + (square(blockY) + square(blockZ);
	
	int x = (-1 * b) / (2 * a);
	int y = squareroot(-1 * (square(b) - 4 * a * c) / (4 * a)); //this is the shortest distance between the BlockPos and the guide line of the cone
	
	int distance = squareroot(square(coneX) + square(coneY) + square(coneZ)) * x;
	int coneRadiusAtDistance = distance * tan(slopeAngle);
	
	return y <= coneRadiusAtDistance;
}