Untitled

void applySensorModelReading(double sensorX, double sensorY, double sensorAngle, double distance, double beamWidthInDegrees) {
    double xa = sensorX + (distance * Math.cos(Math.toRadians(sensorAngle + beamWidthInDegrees / 2)));
    double ya = sensorY + (distance * Math.sin(Math.toRadians(sensorAngle + beamWidthInDegrees / 2)));
    double xb = sensorX + (distance * Math.cos(Math.toRadians(sensorAngle - beamWidthInDegrees / 2)));
    double yb = sensorY + (distance * Math.sin(Math.toRadians(sensorAngle - beamWidthInDegrees / 2)));

    double w = beamWidthInDegrees / Math.sqrt(Math.pow(xa - xb, 2) + Math.pow(ya - yb, 2))/2;

    for (double a = -beamWidthInDegrees / 2; a <= beamWidthInDegrees / 2; a += w) {
        double objX = sensorX + (distance * Math.cos(Math.toRadians(sensorAngle + a)));
        double objY = sensorY + (distance * Math.sin(Math.toRadians(sensorAngle + a)));

        if (isInsideGrid(objX, objY)) {
            setGridValue(objX, objY, (short) 1); 
        }
    }
}