return tmp.t - steer.t;  }  //make this a param from dsc file??    public double getLandmarkAngleVariance(int lm) {      return getDictionary().getDouble("LOCALIZER_LM_ANGLE_VAR");    }  public void clipToMap(Sample s) {    //clip the sample to our map    //		System.out.println("clip to map "+s.toString());    double x = s.data[Sample.x];    double y = s.data[Sample.y];    boolean moved = false;        if (x < left) {      x = left + clippingOffset;      moved = true;    }    else if (x > right) {       x = right - clippingOffset;      moved = true;    }    if (y > top) {       y = top - clippingOffset;      moved = true;    }    else if (y < bottom) {      y = bottom + clippingOffset;      moved = true;    }        if (moved) {                  Sample newSample = gs.generateSample();      //    System.out.println("JRS: ctm: got new samp");      x += newSample.data[Sample.x];      y += newSample.data[Sample.y];            s.data[Sample.x] = x;      s.data[Sample.y] = y;      s.data[Sample.t] += newSample.data[Sample.t];          }  }    public double [] clipToMap(double x, double y, double theta) {    double [] res = new double[3];    boolean moved = false;        if (x < left) {	x = left + clippingOffset;	moved = true;      }      else if (x > right) { 	x = right - clippingOffset;	moved = true;      }      if (y > top) { 	y = top - clippingOffset;	moved = true;      }      else if (y < bottom) {	y = bottom + clippingOffset;	moved = true;      }            if (moved) {			Sample newSample = gs.generateSample();	res[0] = x + newSample.data[Sample.x];	res[1] = y + newSample.data[Sample.y];	res[2] = theta + newSample.data[Sample.t];      }      else {	res[0] = x;	res[1] = y;	res[2] = theta;      }      return res;    }      public boolean onMap(double x, double y) {	if (x < left ||  x > right)	    return false;	//y coord is off	if (y > top || y < bottom)	    return false;	return true;    }    public double [] getMovementDistParams() {	double [] params = new double[6]; //for x y and t movement mn/std		params[0] = mvstep.r; //movement mag mean        // standard deviation 2% distance error after 1m	params[1] = Math.sqrt(Math.pow(.02    ,2.0) * (mvstep.r / 1.0)); //move mag std dev	params[2] = mvstep.t - oldHeading; //move dir mean        // standard deviation .5 degree per .5 meters	params[3] = Math.sqrt(Math.pow(.5 * Math.PI / 180,2.0) * (mvstep.r / 0.5));  //move dir std dev	params[4] = steer.t - oldHeading; //turrent direction	params[5] = Math.sqrt(Math.pow(.5 * Math.PI / 180,2.0) * (mvstep.r / 0.5));		oldMoveR = mvstep.r;	oldMoveT = mvstep.t;	oldHeading = steer.t;	return params;    }  /*---- For LineLocalizationRobot ---*/  public LineSim [] getLines() {    if (lines == null) {      int i;      int num=0;      //landmarks = new SimulatedObject[all_objects.length];      for (i = 0; i < all_objects.length; i++) {	if (all_objects[i] instanceof LineSim) {  	  System.out.println("GLIN: obj "+i+" is line! getP = "+			     all_objects[i].getPosition().toString());	  // landmarks[i] = all_objects[i];	  num++;	}	else {	  System.out.println("GLIN: obj "+i+" is not line...");	  //    landmarks[i] = null;	}      }            lines = new LineSim[num];            for (i = 0,num=0; i < all_objects.length; i++) {	if (all_objects[i] instanceof LineSim) {	  lines[num] = (LineSim) all_objects[i];	  num++;	}      }            System.out.println("JRS: getLines: got "+num+" lines");            return lines;          }    return lines;  }    public Vec2 [] getVisualLines(long timestamp, int channel) {    //only look if new info given....    if ((timestamp > lastVisualLinesTime)|| 	(timestamp == -1)) {            if (timestamp != -1) 	lastVisualLinesTime = timestamp;            //scan our FOV with rays coming from camera to end of view      //from left to right like a radar.  at each line we will       //look at vertRes points to see if they are on a line      //so i guess it's O(horzRes*vertRes*numLines)      //note: because this is simulation, we take adavantage of the fact      //that we know where we are exactly in the world.  for later versions      //we can use that to narrow done the number of lines we look at      /*      double REALLYBIG = 999999;      Vec2 [] left = new Vec2[lines.length]; //hold leftmost points for each line seen      Vec2 [] right = new Vec2[lines.length]; //holld rightmost points for each line seen      for (int i =0;i < lines.length; i++) {	if (steer.t < Math.PI) {	  left[i] = new Vec2(REALLYBIG,0);	  right[i] = new Vec2(-REALLYBIG,0);	}	else {	  left[i] = new Vec2(-REALLYBIG,0);	  right[i] = new Vec2(REALLYBIG,0);	}      }            int total=0; //num lines seen      Vec2 tmpPos = new Vec2(position);      double horzOffset = steer.t - (VISION_FOV_RAD/2);      double testT, testR;      Vec2 test; //this is egocentric vec to point we are checking for lineness      //start at one otherwise we're checking our center      for (int i = 1; i < horzRes; i++) {	testT = horzOffset + ((VISION_FOV_RAD)/(double)horzRes)*i;	for (int j =1; j < vertRes; j++) {	  test = new Vec2(0,0);	  test.setr((VISION_RANGE)/(double)vertRes*j);	  test.sett(testT);	  test.add(tmpPos); //check on lineness from global center	  for (int k = 0; k < lines.length; k++) {	    if (lines[k].pointOnLine(test)) {	      //ok so we can see a point on this line...	      System.out.println("GVL: can see line "+k+" at "+test.toString());	      	      if (steer.t < Math.PI) {		if (test.x < left[i].x)		  left[i].setx(test.x);		if (test.x > right[i].x)		  right[i].setx(test.x);	      }	      else {		if (test.x > left[i].x)		  left[i].setx( test.x);		if (test.x < right[i].x)		  right[i].setx(test.x);	      }	      total++;	    }	  }	}      }            linesRes = new Vec2[total*2]; //hold the endpoints      int count =0;      i      for (int i =0; i < lines.length; i++) {	if (Math.abs(left[i].x) != REALLYBIG) {	  linesRes[count] =  new Vec2(left[i].x, lines[i].evaluate(left[i].x));	  linesRes[count+1] = new Vec2(right[i].x, lines[i].evaluate(right[i].x));	  linesRes[count].sub(tmpPos);	  linesRes[count+1].sub(tmpPos);	  count += 2;	}      }            return linesRes;      */            Vec2 beginSeen, endSeen;      linesRes = new Vec2[lines.length*2];      boolean haveSeen;      //for each line, we look and see if we can find who's looking at it...      //this could be done globally by the lines to save time (for multiple robots)            for (int i =0; i < lines.length; i++) {	Vec2 start = lines[i].getStart();	Vec2 end = lines[i].getEnd();		//fromRobot is the vector pointing to one end of a line we are testing	//for visibility	Vec2 fromRobot = new Vec2(start);	fromRobot.sub(position);	Vec2 tmp = new Vec2(end);	tmp.sub(start);	double totalLength = tmp.r;	int numDiv = (int) (totalLength / lineRes); //number of divisions in this line...	haveSeen = false; //havent seen this line yet	beginSeen = null;	endSeen = null;	int j;	for (j = 0; j < numDiv; j++) {	  tmp.setr( j*lineRes );	  fromRobot.add(tmp);	  if ((fromRobot.r < SimpleCye.VISION_RANGE) &&	      (Math.abs(Units.BestTurnRad(steer.t, fromRobot.t)) <	       (SimpleCye.VISION_FOV_RAD/2))) {	    //we can see this point on the line...	    /*   System.out.println("seen line start "+start.toString()+" end "+end.toString());	    System.out.println("GVL: fR.r = "+fromRobot.r+" s.t = "+steer.t+			       " fR.t = "+fromRobot.t+" ang = "+Math.abs(Units.BestTurnRad(steer.t, fromRobot.t)));	    System.out.println("pos = "+position.toString());	    */	    //ok we start to see this line	    if (!haveSeen) {	     	      haveSeen = true;	      beginSeen = new Vec2(fromRobot);	      	    	    }	  }	  else {	    //if havent seen it and seen before, stop seeing	    if (haveSeen) {	      haveSeen = false;	      //point back to start of line	      fromRobot.sub(tmp);	      //the previous point on the line was the last one we could see	      tmp.setr((j-1)*lineRes);	      fromRobot.add(tmp);	      endSeen = new Vec2(fromRobot);	      //point back to start of the line	      fromRobot.sub(tmp);	      //      System.out.println("end 1 j -1 = "+(j-1));	      //     System.out.println("GVL: seeting endpoint on line "+i+" ts ="+timestamp);	      Vec2 seg = new Vec2(endSeen);	      seg.sub(beginSeen);	      //    System.out.println("GVL: seg = "+seg.toString());	      	      break; //no need to finish this	    }	  }	  fromRobot.sub(tmp);	}	if (haveSeen) {	  //the line continued past of FOV	  //the previous point was the last one seen	  tmp.setr((j-1)*lineRes);	  fromRobot.add(tmp);	  endSeen = new Vec2(fromRobot);	  //	  System.out.println("end 2 j-1 = "+(j-1));		  	}	//set the vector to the first point we see	if (beginSeen != null) 	  linesRes[i*2] = new Vec2(beginSeen);	else	  linesRes[i*2] = null;	//set the vector to the end point of visible part of the line	if (endSeen != null) {	  linesRes[2*i+1] = new Vec2(endSeen);	  System.out.println("LINES: ("+linesRes[i*2].x+", "+linesRes[i*2].y+") <-> ("+			     linesRes[2*i+1].x+", "+linesRes[2*i+1].y+")");	}	else {	  linesRes[2*i+1]= null;	}	      }      //these are pointing egocentric to endpoints of the line??      return linesRes;          }    return linesRes;      }}