void GoToLandmark(Thing landmark, Segment s)
{
Segment s1 = Module2DModel.SegmentFromUKSThing(landmark.References[0].T);
Module2DModel.OrderSegment(s1);
float ratio = s1.P1.R / (s1.Length);
PointPlus target = new PointPlus
{
X = s.P1.X + (s.P2.X - s.P1.X) * ratio,
Y = s.P1.Y + (s.P2.Y - s.P1.Y) * ratio,
};
MoveTo(target, .2f);
}
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);
}
}
}
}
public override void Fire()
{
Init(); //be sure to leave this here
if (GetNeuronValue("Auto") == 0)
{
return;
}
goToGoal = GetNeuronValue("Goal") == 1;
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
if (mBehavior == null)
{
return;
}
Module2DModel mModel = (Module2DModel)FindModleu(typeof(Module2DModel));
if (mModel is null)
{
return;
}
ModuleEvent mEvent = (ModuleEvent)FindModleu(typeof(ModuleEvent));
if (mEvent == null)
{
return;
}
ModuleUKSN UKS = (ModuleUKSN)FindModleu(typeof(ModuleUKSN));
if (UKS == null)
{
return;
}
if (GetNeuronValue("ModuleBehavior", "Done") == 0)
{
return;
}
mModel.GetSegmentsFromUKS();
IList <Thing> segments = mModel.GetUKSSegments();
if (segments.Count == 0)
{
return;
}
Thing t = segments[0]; //TODO: this only works with just a single item in the UKS
//figure out if any motion occured
Segment s = Module2DModel.SegmentFromUKSThing(t);
Module2DModel.OrderSegment(s);
if (GetNeuronValue("E0") == 1)
{
GoToLandmark(UKS.Labeled("E0").References[0].T, s);
doPush = 2;
doBackOff = true;
return;
}
if (GetNeuronValue("E1") == 1)
{
GoToLandmark(UKS.Labeled("E1").References[0].T, s);
doPush = 2;
doBackOff = true;
return;
}
if (GetNeuronValue("E2") == 1)
{
GoToLandmark(UKS.Labeled("E2").References[0].T, s);
doPush = 2;
doBackOff = true;
return;
}
if (doPush != 0)
{
if (doPush == 2)
{
Push(s);
}
doPush--;
return;
}
if (doFaceSegment)
{
DoFaceSegment(s);
doFaceSegment = false;
return;
}
Segment s1;
if (lastPosition == null) //create objects to keep track of the target and last position
{
s1 = s.Clone();
lastPosition = mModel.AddSegmentToUKS(s1);
lastPosition.Label = "LastPosition";
lastPosition.RemoveParent(UKS.Labeled("Segment"));
lastPosition.AddParent(UKS.Labeled("SSegment"));
Module2DSim mSim = (Module2DSim)FindModleu(typeof(Module2DSim));
if (mSim is null)
{
return;
}
Segment motionTarget = mSim.GetMotionTarget();
if (motionTarget == null)
{
motionTarget = new Segment();
motionTarget.P1 = new PointPlus(4, 1.5f);
motionTarget.P2 = new PointPlus(4, -2.5f);
motionTarget.theColor = (ColorInt)0xff;
}
endTarget = mModel.AddSegmentToUKS(motionTarget);
endTarget.Label = "EndTarget";
//endTarget.RemoveParent(UKS.Labeled("Segment"));
//endTarget.AddParent(UKS.Labeled("SSegment"));
}
else
{
s1 = Module2DModel.SegmentFromUKSThing(lastPosition);
}
//get motion from subtracting and then updating last position
Angle rotation = s.Angle - s1.Angle;
if (rotation > PI / 2)
{
rotation = PI - rotation;
}
if (rotation < -PI / 2)
{
rotation = PI + rotation;
}
Motion motion = new Motion()
{
P = (Point)s.MidPoint.V - s1.MidPoint.V,
rotation = rotation,
};
lastPosition.References[0].T.V = s.P1.Clone();
lastPosition.References[1].T.V = s.P2.Clone();
if (Abs(motion.R) > .01 || Abs(motion.rotation) > .05 && !goToGoal && !doBackOff)
{
//check for existing Event
Thing currentEvent = MostLikelyEvent(t);
if (currentEvent == null)
{ //add new Event
Thing lm1 = mEvent.CreateLandmark(new List <Thing>()
{
t
});
Thing t1 = mEvent.CreateEvent(lm1);
Thing t3 = UKS.AddThing("m" + motionCount++, UKS.Labeled("Motion"), motion);
mEvent.AddOutcomePair(t1, UKS.GetOrAddThing("Push", "Action"), t3);
}
return;
}
if (doBackOff)
{
DoBackOff(s);
doBackOff = false;
doFaceSegment = true;
return;
}
if (goToGoal)
{
if (endTarget != null)
{
Segment s2 = Module2DModel.SegmentFromUKSThing(endTarget);
GoToGoal(s, s2);
return;
}
}
Explore(s);
}
