Stochastic Geometry Random tangents

/////////////////////////////////////////////////////////////////////
//
// TACAN - TeleAutonomous Control And Navigation
//
// Mark Dennehy, 93369425
// S.S. C.Eng.
// Final Year Project 1996/7
//
//////////////////////////////////////////////////////////////////////
//
// PolarHistogram.java
//    implements the polar histogram
//  -----------------
// $Date: 1997/04/03 21:49:54 $
// $Revision: 2.0 $
// $State: Stable $
//
//////////////////////////////////////////////////////////////////////

package Tacan.RLC;

import Tacan.util.LogFile;
import Tacan.util.MathUtil;
import java.util.*;
import java.io.*;
import java.awt.event.*;

/**
 * The Polar Histogram of the Certainty value grid. This is effectively a
 * polar graph of obstacle density.
 *
 *@author
 *@version
 *@see Tacan.RLC.CVGrid
 *@see java.util.Observer
 *@see java.awt.event.ActionListener
 **/
class PolarHistogram extends Observable implements Observer, ActionListener
{
   private final static double A = 22;
   private final static double B = 1;
   private final static int maxFreeSectorSize = 18;

   private int tmpPH[];
   private RLC rlc_;
   private int no_of_segments_;
   private LogFile log_;
   private LogFile data_;
   private Vector segment_terms_[];

   /**
    * The current Polar Histogram values
    **/
   protected int PH_[];
   
   /**
    * The Local Autonomy Control graph values
    **/
   protected double LAC_[];

   /**
    * The current maximum value in the Polar Histogram (for normalisation)
    **/
   protected int PH_max = 0;

   /**
    * The current maximum value in the Local Autonomy Control graph (for normalisation)
    *@see
    **/
   protected double LAC_max = 0.0;

   /**
    * The threshold value for deciding what path is free and what is not
    **/
   protected int PH_threshold = 100;

   /**
    * The maximum network lag permitted before full local autonomy is granted (in seconds)
    **/
   public final static double maxNetLag = 10;

   /**
    * Constructor
    *
    *@param rlc The Robot Local controller instance associated with this graph
    **/
   protected PolarHistogram(RLC rlc)
   {
      rlc_ = rlc;
      PH_max = 0;
      segment_terms_ = new Vector[rlc.robot_config.PH_RES];
      no_of_segments_ = rlc.robot_config.PH_RES;
      PH_ = new int[no_of_segments_];
      tmpPH = new int[no_of_segments_];
      LAC_ = new double[no_of_segments_];
      log_ = new LogFile("Polar.log");
      data_ = new LogFile("Polar.dat");

      for (int i = 0; i<rlc.robot_config.PH_RES; i++)
         segment_terms_[i] = new Vector();

      generateSegmentTerms();
   }

   public int getThreshold()
   {
      return PH_threshold;
   }
   
   public void setThreshold(int i)
   {
      PH_threshold = i;
   }

   public void actionPerformed(ActionEvent e)
   {
      if(e.getActionCommand() == "Reset")
         reset();
   }

   protected void reset()
   {
      PH_ = new int[no_of_segments_];
   }

   private void generateSegmentTerms()
   {
      double theta, r;
      int x,y;
      int seg_no;
      double res = 360/no_of_segments_;
      int max_x = (int)(rlc_.robot_config.ACTIVE_WINDOW_SIZE/2) ;
      int max_y = max_x;
      SectorCell cell;

      System.out.print("Calculating segment terms :  ");
      System.out.flush();

      for (x = -max_x ; x <= max_x ; x++)
      {
         for (y =-max_y  ; y<=max_y  ; y++)
            if ((x!=0) || (y!=0)) // Avoid (0,0)
            {
               cell = new SectorCell(0,0,0);
               theta = MathUtil.rad2deg(Math.atan2(y,x)); //No, not a typo, atan2 takes (x,y) as (y,x)
               theta = (theta < 0) ? 360 + theta : theta ;
               
               r = Math.sqrt((x*x)+(y*y));
               cell.x = x;
               cell.y = y;
               cell.term = A - B*r;
               seg_no = (int)(theta/res);
               segment_terms_[seg_no].addElement(cell);
            }
         System.out.print(".");
         System.out.flush();
      }

      for (int i = 0; i<no_of_segments_; i++)
         segment_terms_[i].trimToSize();

      System.out.println(" Done.");
      System.out.flush();
   }

   public synchronized void update(Observable o,Object arg)
   {
      Thread.currentThread().setPriority(Thread.MAX_PRIORITY);
      Enumeration sector;
      int sector_sum;
      SectorCell cell = new SectorCell(0,0,0);
      int x_offset = (int)(rlc_.robot_config.RLC_LOCAL_MAP_MAX/2);
      int y_offset = (int)(rlc_.robot_config.RLC_LOCAL_MAP_MAX/2);
      int cv;

      for (int i = 0; i < no_of_segments_; i++)
         {
            sector_sum = 0;
            sector = segment_terms_[i].elements();
            while(sector.hasMoreElements())
               {
                  cell = (SectorCell)sector.nextElement();
                  cv = rlc_.cv_grid.local_map[cell.x+x_offset][cell.y+y_offset];
                  sector_sum += cv*cv*cell.term;
               }
            tmpPH[i] = sector_sum;
         }

      smooth(5);

      setChanged();
      notifyObservers();
      Thread.currentThread().setPriority(Thread.NORM_PRIORITY);
   }

   /**
    * Averaging filter on Polar Histogram
    *    The values for the PH can vary widely, so a smoothing filter is required
    *
    *@param filterSize The number of samples to use to smooth the graph
    *@return void
    */
   protected synchronized void smooth(int filterSize)
   {
      int sum = 0;
      PH_max = 0;

      if ((filterSize%2)==0) //If even, make odd
         filterSize++;

      int df = (int)((double)filterSize/2);

      for(int i = 0; i < no_of_segments_; i++)
      {
         sum = 0;
         for (int j = -df; j <= df; j++)           sum += tmpPH[(df+i+j)%no_of_segments_];         sum = (int)((double)sum/filterSize);         PH_[(i+df)%no_of_segments_] = sum;        PH_max = (sum > PH_max) ? sum : PH_max;
      }
   }
   
   public synchronized void LACupdate(int t,int desired_sector)
   {
      double b = (t<maxNetLag) ? (double)t/maxNetLag : 1.0 ;
      LAC_max = 0;

      double slope = (1-b)/(no_of_segments_/2);

      for (int i = 0 ; i < no_of_segments_ ; i++)
         {
         LAC_[(desired_sector+i)%no_of_segments_] = b;
         LAC_[(desired_sector+i)%no_of_segments_] += (i < (int)(no_of_segments_/2)) ? (double)i*(slope) : (-slope)*(double)(i-no_of_segments_);          LAC_max = (LAC_[i] > LAC_max) ? LAC_[i] : LAC_max;
         }
   }

   public int clearPath(int bearing)
   {
      int res = (int)360.0/no_of_segments_;
      int desired_sector = (int)(bearing/res);
      int b;
      int l_centre =1;
      int l_width = 1;
      int r_centre =1;
      int r_width = 1;

      LACupdate(3,desired_sector);

      if ((PH_[desired_sector] * LAC_[desired_sector]) < PH_threshold)
         return bearing;
      else
      {
         //Scan Left
         for(int a = 1; a < (no_of_segments_/2) ; a++)
            //Start of first free sector
            if ((PH_[(a+desired_sector)%no_of_segments_] * LAC_[(a+desired_sector)%no_of_segments_]) < PH_threshold)
            {
               //For up to maximum sector width
               for (l_width = 1; l_width <= maxFreeSectorSize; l_width++)                    //Look for end of free Sector                   if ((PH_[(a+l_width+desired_sector)%no_of_segments_] * LAC_[(a+l_width+desired_sector)%no_of_segments_]) > PH_threshold)
                     {
                        //Set centre of sector
                        l_centre = a+(int)(l_width/2);
                        break;
                     }
               break;
            }

         //Scan Right
         for(int a = no_of_segments_-1; a > (no_of_segments_/2) ; a--)
            //Start of first free sector
            if ((PH_[(a+desired_sector)%no_of_segments_] * LAC_[(a+desired_sector)%no_of_segments_]) < PH_threshold)
            {
               //For up to maximum sector width
               for (r_width = 1; r_width <= maxFreeSectorSize; r_width++)                    //Look for end of free Sector                   if ((PH_[a-r_width] * LAC_[a-r_width]) > PH_threshold)
                     {
                        //Set centre of sector
                        r_centre = a-(int)(r_width/2);
                        break;
                     }
               break;
            }

         //Choose left or right, weighing size of sector with deflection
         b = ((l_width/l_centre) > (r_width/(no_of_segments_ - r_centre))) ?  l_centre : r_centre ;  
         b += desired_sector;
         b %= no_of_segments_;
         b *= res;
         
         return b;
      }
   }

   public String toString()
   {
      String s = "[ ";
      for (int i = 0;i<no_of_segments_;i++)
         s += String.valueOf(PH_[i]) + " ";
      s += "]";
      return s;
   }
}