Untitled

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

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

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

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