Untitled

public void applySensorModelReading(double sensorX, double sensorY, int sensorAngle, double distance, double beamWidthInDegrees, double distanceErrorAsPercent) {
    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)));
      System.out.println("Test");
      if (isInsideGrid(objX, objY)){
        System.out.println("test from inside apply sensor model reading");
        setGridValue(objX,objY, (short)1);
      }
    }
    // double INC = 0.25;
    // for (double r = distance * (1-distanceErrorAsPercent); r < distance * (1+distanceErrorAsPercent); r+=INC){
      // 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 + (r * Math.cos(Math.toRadians(sensorAngle + a)));
        // double objY = sensorY + (r * Math.sin(Math.toRadians(sensorAngle + a)));
        System.out.println("Test");
        // if (isInsideGrid(objX, objY)){
          System.out.println("test from inside apply sensor model reading");
          // setGridValue(objX,objY, (short)1);
        // }
      // }
    // }
  }