private void FindPointsOfInterest() { Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel)); if (nmModel == null) { return; } LBoundaries.Clear(); RBoundaries.Clear(); FindMonocularBoundaries(0, LBoundaries); FindMonocularBoundaries(1, RBoundaries); FindBinocularBoundaries(); FindAreasOfColor(); //curArea.t being null means this area is not in the model...just add it foreach (Area curArea in areas) { if (curArea.t == null && curArea.theColor != 0) { //add the segment to the model curArea.t = nmModel.AddSegmentFromVision(curArea.PL, curArea.PR, curArea.theColor, viewChanged == 0); } } for (int i = 0; i < areas.Count; i++) { Area curArea = areas[i]; if (i < areas.Count - 1) { Area nextArea = areas[i + 1]; //the case of two adjacent colored boundaries means there are adjoining or occluding areas //if occluding, do not update the (possibly) hidden point(s) if (curArea.theColor != 0 && nextArea.theColor != 0 && curArea.t != null && nextArea.t != null) //&& curArea.PR.R == nextArea.PL.R && curArea.PR.Theta == nextArea.PL.Theta) { Segment curS = Module2DModel.SegmentFromUKSThing(curArea.t); Module2DModel.OrderSegment(curS); Segment nextS = Module2DModel.SegmentFromUKSThing(nextArea.t); Module2DModel.OrderSegment(nextS); //is aa.PL in front of prevSegment (the little correction hides an occlusion problem where the endpoints nearly match if (nextArea.PL.Theta > curS.P1.Theta - Rad(2) && nextArea.PL.Theta < curS.P2.Theta + Rad(2)) { float segDistAtPoint = curS.P1.R; float dr1 = (curS.P2.R - curS.P1.R); float dtp = (curS.P1.Theta - nextArea.PL.Theta); float dtt = (curS.P1.Theta - curS.P2.Theta); segDistAtPoint += dr1 * dtp / dtt; if (nextArea.PL.R < segDistAtPoint) { curArea.PRHidden = true; } } //is prevArea.PR in front of aaSegment if (curArea.PR.Theta > nextS.P1.Theta - Rad(2) && curArea.PR.Theta < nextS.P2.Theta + Rad(2)) { float segDistAtPoint = nextS.P1.R + (nextS.P2.R - nextS.P1.R) * (nextS.P1.Theta - curArea.PR.Theta) / (nextS.P1.Theta - nextS.P2.Theta); if (curArea.PR.R < segDistAtPoint) { nextArea.PLHidden = true; } } } } } //check for single boundaries and update them in the model foreach (Area curArea in areas) { //if the model data already exists, update points if (curArea.t != null) { if (!curArea.PLHidden) { nmModel.UpdateEndpointFromVision(curArea.PL, curArea.theColor, viewChanged == 0); } if (!curArea.PRHidden) { nmModel.UpdateEndpointFromVision(curArea.PR, curArea.theColor, viewChanged == 0); } } } }