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);
}
}
}
}