Thing MostLikelyEvent(Thing currentEvent)
{
Thing retVal = null;
ModuleUKSN UKS = (ModuleUKSN)FindModleu(typeof(ModuleUKSN));
if (UKS == null)
{
return(retVal);
}
Segment s1 = Module2DModel.SegmentFromUKSThing(currentEvent.References[0].T);
IList <Thing> events = UKS.Labeled("Event").Children;
foreach (Thing t in events)
{
if (t.References.Count > 0)
{
Thing lm = t.References[0].T;
if (lm.References.Count > 0)
{
Thing lms = lm.References[0].T;
Segment s2 = Module2DModel.SegmentFromUKSThing(lms);
if (s1.MidPoint.Near(s2.MidPoint, 0.2f))
{
retVal = t;
}
}
}
}
return(retVal);
}
public Thing CreateLandmark(List <Thing> near)
{
//a landmark is a collection of nearby segments
//these must be cloned so they can be fixed in position rather than being adjusted relative to Sallie's position
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (UKS is null)
{
return(null);
}
Thing newLandmark = UKS.AddThing("Lm" + landmarkCount++.ToString(), "Landmark");
foreach (Thing t in near)
{
Segment s = Module2DModel.SegmentFromUKSThing(t);
Thing P1 = UKS.AddThing("lmp" + pointCount++.ToString(), "Point"); //These points are not included in the model and do don't move with poin of view
P1.V = s.P1.Clone();
Thing P2 = UKS.AddThing("lmp" + pointCount++.ToString(), "Point");
P2.V = s.P2.Clone();
Thing S = UKS.AddThing("l" + t.Label.ToString(), "SSegment");
S.AddReference(P1);
S.AddReference(P2);
S.AddReference(t.References[2].T);//the color
newLandmark.AddReference(S);
}
return(newLandmark);
}
private Angle GetTotalAngleError(List <Thing> near)
{
if (lastLandmark == null)
{
return(10000);
}
Angle totalAngleError = 0;
for (int i = 0; i < lastLandmark.References.Count; i++)
{
Segment s1 = Module2DModel.SegmentFromUKSThing(lastLandmark.References[i].T);
foreach (Thing t1 in near)
{
//does it match one of the segments presently near me?
Segment s2 = Module2DModel.SegmentFromUKSThing(t1);
if (s1.theColor == s2.theColor)
{
Angle m1 = s1.MidPoint.Theta;
Angle m2 = s2.MidPoint.Theta;
Angle angle = m1 - m2;
totalAngleError += Abs(angle);
}
}
}
return(totalAngleError);
}
//for debugging it is handy to bypass the exploration to establish the internal model...just set it
public void SetModel()
{
MainWindow.SuspendEngine();
Initialize();
Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (nmModel == null)
{
return;
}
//clear out any existing
nmModel.Initialize();
for (int i = 0; i < objects.Count; i++)
{
PointPlus P1 = new PointPlus()
{
P = objects[i].P1
};
PointPlus P2 = new PointPlus()
{
P = objects[i].P2
};
int theColor = Utils.ColorToInt(objects[i].theColor);
nmModel.AddSegmentFromVision(P1, P2, theColor, false);
}
MainWindow.ResumeEngine();
}
private void CorrectModelPosition(Module2DModel nmModel, ModuleBehavior nmBehavior, Thing best, List <Thing> near)
{
//locations are not very accurate and accumulate errors
//whenever we encounter a landmark, we update the orientation/location of the model to correct errors
Segment s2 = Module2DModel.SegmentFromUKSThing(near[0]);
Segment s1 = null;
foreach (Link l in best.References)
{
Segment s = Module2DModel.SegmentFromUKSThing(l.T);
if (s.theColor == s2.theColor)
{
s1 = s; break;
}
}
if (s1 != null)
{
PointPlus m1 = s1.MidPoint;
PointPlus m2 = s2.MidPoint;
float deltaX = m1.X - m2.X;
float deltaY = m1.Y - m2.Y;
PointPlus a1 = new PointPlus()
{
P = (Point)(s1.P1.V - s1.P2.V)
};
PointPlus a2 = new PointPlus()
{
P = (Point)(s2.P1.V - s2.P2.V)
};
Angle deltaTheta = a1.Theta - a2.Theta;
nmModel.Move(-deltaX, -deltaY);
nmBehavior.TurnTo(deltaTheta);
}
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
double[] rotation = { -Math.PI / 2, -Math.PI / 24, 0, Math.PI / 24, Math.PI / 2 };
float direction = 0;
for (int i = 0; i < na.Width; i++)
{
if (na.GetNeuronAt(i, 0).LastCharge > 0.9 || na.GetNeuronAt(i, 0).LastCharge > 0.9 && i < rotation.Length)
{
direction = (float)rotation[i];
}
}
if (na.GetNeuronAt(2, 0).CurrentCharge != 0)
{
direction = na.GetNeuronAt(2, 0).CurrentCharge;
na.GetNeuronAt(2, 0).SetValue(0);
}
ModuleRealityModel mrm = (ModuleRealityModel)FindModuleByType(typeof(ModuleRealityModel));
if (mrm != null && direction != 0)
{
mrm.Rotate(20);
}
Module2DSim m2D = (Module2DSim)FindModuleByType(typeof(Module2DSim));
if (m2D != null && direction != 0)
{
m2D.Rotate(direction);
}
Module3DSim m3D = (Module3DSim)FindModuleByType(typeof(Module3DSim));
if (m3D != null && direction != 0)
{
m3D.Rotate(direction);
}
Module2DModel m2DModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (m2DModel != null && direction != 0)
{
m2DModel.Rotate(direction);
}
Module2DVision m2DVision = (Module2DVision)FindModuleByType(typeof(Module2DVision));
if (m2DVision != null && direction != 0)
{
m2DVision.ViewChanged();
}
}
private static void FindBestLandmarkMatch(ModuleUKSN UKS, ref Thing best, ref float bestDist, List <Thing> near)
{
//searching each spatial Event
best = null;
bestDist = 1000;
if (UKS == null)
{
return;
}
if (UKS.Labeled("Landmark") == null)
{
return;
}
//landmark segments are sorted, closest segment first
//closer segments are more important
List <Thing> landmarks = UKS.Labeled("Landmark").Children;
foreach (Thing landmark in landmarks)
{
float totalDist = 0;
//each reference in that landmark
int foundCount = 0; //must match several items to count as a hit
int linkNumber = 0;
foreach (Link L1 in landmark.References)
{
Segment s1 = Module2DModel.SegmentFromUKSThing(L1.T);
float d1 = (float)Utils.FindDistanceToSegment(s1);
int nearNumber = 0;
foreach (Thing t1 in near)
{
//does it match one of the segments presently near me?
Segment s2 = Module2DModel.SegmentFromUKSThing(t1);
float d2 = (float)Utils.FindDistanceToSegment(s2);
if (s1.theColor == s2.theColor)
{
foundCount++;
float dist = (float)Abs(d1 - d2);
dist *= Abs(nearNumber - linkNumber) + 1;
totalDist += dist;
}
nearNumber++;
}
linkNumber++;
}
if (totalDist < bestDist && foundCount == near.Count)
{
best = landmark;
bestDist = totalDist;
}
}
}
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);
}
//an area must appear in both eyes to count
//partial areas as the edge of the field of view will have only one endpoint
//this relies on having only one area per color in the visual field
private void FindAreasOfColor()
{
areas.Clear();
if (boundaries.Count == 0)
{
return;
}
BinocularBoundary bb1 = boundaries[0];
for (int i = 1; i < boundaries.Count; i++)
{
BinocularBoundary bb2 = boundaries[i];
if (bb1.theColor == bb2.theColor)
{
Area aa = new Area()
{
PL = bb1.p,
PR = bb2.p,
lChanged = bb1.changed,
RChanged = bb2.changed,
theColor = bb1.theColor,
};
//aa.angleFromTexture = GetAngleFromTexture(aa);
Segment s = new Segment()
{
P1 = aa.PL, P2 = aa.PR, theColor = aa.theColor,
};
Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (nmModel != null)
{
aa.t = nmModel.MostLikelySegment(s);
}
areas.Add(aa);
}
bb1 = bb2;
}
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
Module2DModel naModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (naModel == null)
{
return;
}
for (int i = 0; i < na.Height; i++)
{
//neurons: 0:touch 1:antAngle 2:antDistance 3: sensedLineAngle 4: conf1 5: len1 6: conf2 7: len2 8: touch-ended 9: modelchanged
if (na.GetNeuronAt(0, i).CurrentCharge == 0)
{
continue;
}
float antDist = na.GetNeuronAt(1, i).CurrentCharge;
float antAngle = na.GetNeuronAt(2, i).CurrentCharge;
float lineAngle = na.GetNeuronAt(3, i).CurrentCharge;
float p1IsEndpt = na.GetNeuronAt(4, i).CurrentCharge;
float l1 = na.GetNeuronAt(5, i).CurrentCharge;
float p2IsEndpt = na.GetNeuronAt(6, i).CurrentCharge;
float l2 = na.GetNeuronAt(7, i).CurrentCharge;
float mR = na.GetNeuronAt(9, i).CurrentCharge;
float mTheta = na.GetNeuronAt(10, i).CurrentCharge;
float mPhi = na.GetNeuronAt(11, i).CurrentCharge;
PointPlus motion = new PointPlus()
{
R = mR, Theta = mTheta, Conf = mPhi
};
//create the line segment (all coordinates relative to self)
PointPlus antennaPos = new PointPlus()
{
R = antDist, Theta = antAngle
};
float lineAngleAbs = antAngle - lineAngle;
PointPlus pv1 = new PointPlus()
{
R = l1, Theta = (float)Math.PI + lineAngleAbs
};
PointPlus pv2 = new PointPlus()
{
R = l2, Theta = lineAngleAbs
};
Point P1 = antennaPos.P + pv1.V;
Point P2 = antennaPos.P + pv2.V;
PointPlus P1P = new PointPlus()
{
P = P1, Conf = 1 - p1IsEndpt
};
PointPlus P2P = new PointPlus()
{
P = P2, Conf = 1 - p2IsEndpt
};
bool modelChanged = naModel.AddSegmentFromTouch(P1P, P2P, motion, i);
}
}
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);
}
public override bool Draw(bool checkDrawTimer)
{
if (!base.Draw(checkDrawTimer))
{
return(false);
}
Module2DModel parent = (Module2DModel)base.ParentModule;
theCanvas.Children.Clear();
Point windowSize = new Point(theCanvas.ActualWidth, theCanvas.ActualHeight);
Point windowCenter = new Point(windowSize.X / 2, windowSize.Y / 2);
float scale = (float)Math.Min(windowSize.X, windowSize.Y) / 12;
if (scale == 0)
{
return(false);
}
TransformGroup tg = new TransformGroup();
tg.Children.Add(new RotateTransform(90));
tg.Children.Add(new ScaleTransform(scale, -scale, 0, 0));// windowCenter.X, windowCenter.Y));
tg.Children.Add(new TranslateTransform(windowCenter.X, windowCenter.Y));
theCanvas.RenderTransform = tg;
//draw an origin point
theCanvas.Children.Add(new Line
{
X1 = -.20,
X2 = .20,
Y1 = 0,
Y2 = 0,
StrokeThickness = 1 / scale,
Stroke = Brushes.Black
});
theCanvas.Children.Add(new Line
{
X1 = 0,
X2 = 0,
Y1 = -.20,
Y2 = .20,
StrokeThickness = 1 / scale,
Stroke = Brushes.Black
});
//draw possible points;
try
{
foreach (Thing t in parent.GetUKSPoints() ?? Enumerable.Empty <Thing>())
{
if (t.V is PointPlus P1 && !float.IsInfinity(P1.X) && !float.IsInfinity(P1.Y))
{
theCanvas.Children.Add(new Line
{
X1 = P1.X,
X2 = P1.X,
Y1 = P1.Y,
Y2 = P1.Y,
StrokeThickness = 3 / scale,
StrokeEndLineCap = PenLineCap.Round,
StrokeStartLineCap = PenLineCap.Round,
// Stroke = new SolidColorBrush(P1.TheColor)
Stroke = new SolidColorBrush(Colors.Orange)
});
}
}
}
catch { }
//draw the objects'
try
{
foreach (Thing t in parent.GetUKSSegments() ?? Enumerable.Empty <Thing>())
{
Segment segment = Module2DModel.SegmentFromUKSThing(t);
if (float.IsNaN(segment.P1.R))
{
continue;
}
Color theColor = Utils.IntToColor(segment.theColor);
Point P1 = segment.P1.P;
Point P2 = segment.P2.P;
Point P1P = P1 + (P2 - P1) * segment.P1.Conf / 4; // .2;
Point P2P = P1 + (P2 - P1) * (1 - segment.P2.Conf / 4); // .8;
theCanvas.Children.Add(new Line
{
X1 = P1.X,
X2 = P2.X,
Y1 = P1.Y,
Y2 = P2.Y,
StrokeThickness = 4 / scale,
Stroke = new SolidColorBrush(theColor),
});
if (segment.P1.Conf != 0)
{
theCanvas.Children.Add(new Line
{
X1 = P1.X,
X2 = P1P.X,
Y1 = P1.Y,
Y2 = P1P.Y,
StrokeThickness = 4 / scale,
Stroke = new SolidColorBrush(Colors.White),
});
}
if (segment.P2.Conf != 0)
{
theCanvas.Children.Add(new Line
{
X1 = P2.X,
X2 = P2P.X,
Y1 = P2.Y,
Y2 = P2P.Y,
StrokeThickness = 4 / scale,
Stroke = new SolidColorBrush(Colors.White),
});
}
}
}
catch { }
if (parent.imagining)
{
//draw any imagined objects
for (int i = 0; i < parent.imagination.Count; i++)
{
Color theColor = Utils.IntToColor(parent.imagination[i].theColor);
Point P1 = parent.imagination[i].P1.P;
Point P2 = parent.imagination[i].P2.P;
Point P1P = P1 + (P2 - P1) * .2;
Point P2P = P1 + (P2 - P1) * .8;
theCanvas.Children.Add(new Line
{
X1 = P1.X,
X2 = P2.X,
Y1 = P1.Y,
Y2 = P2.Y,
StrokeThickness = 4 / scale,
Stroke = new SolidColorBrush(theColor),
Opacity = .5
});
}
LinearGradientBrush lb = new LinearGradientBrush();
lb.StartPoint = new Point(0, 1);
lb.EndPoint = new Point(0, 0);
lb.GradientStops.Add(new GradientStop(Colors.Transparent, 0.0));
lb.GradientStops.Add(new GradientStop(Colors.Transparent, 0.4));
lb.GradientStops.Add(new GradientStop(Colors.White, .76));
lb.GradientStops.Add(new GradientStop(Colors.White, 1.0));
Rectangle r = new Rectangle()
{
Width = 40,
Height = 40,
Opacity = .75,
Fill = lb
};
Canvas.SetTop(r, -20);
Canvas.SetLeft(r, -20);
theCanvas.Children.Add(r);
}
return(true);
}
public override void Fire()
{
Init(); //be sure to leave this here
//get the external references
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (UKS == null)
{
return;
}
Module2DModel nmModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (nmModel == null)
{
return;
}
ModuleBehavior nmBehavior = (ModuleBehavior)FindModuleByType(typeof(ModuleBehavior));
if (nmBehavior == null)
{
return;
}
ModuleEvent mEvent = (ModuleEvent)FindModuleByType(typeof(ModuleEvent));
if (mEvent == null)
{
return;
}
//check on the various input neurons...
//check for a goal selection
foreach (Neuron n in na.Neurons())
{
if (n.Fired() && n.Label.IndexOf("c") == 0 && n.Model == Neuron.modelType.Std)
{
Thing newTarget = UKS.Labeled(n.Label);
if (newTarget == currentTarget)
{
currentTarget = null;
}
else
{
currentTarget = newTarget;
}
currentTargetReached = null;
break;
}
}
//check for operating mode
auto = (GetNeuronValue(null, "Auto") == 1) ? true : false;
//don't do anything while a behavior is in progress
if (GetNeuronValue("ModuleBehavior", "Done") == 0)
{
return;
}
//is there an existing landmark? Do I recognize I'm near a spot I was before?
//this only calculates "R" so it is rotation-independent
Thing best = null;
float bestDist = 1000;
List <Thing> near = nmModel.NearbySegments(3);
FindBestLandmarkMatch(UKS, ref best, ref bestDist, near);
nmModel.FireVisibleObjects(); //not really needed
if (bestDist < .2f) //are we near a landmark we've been at before?
{
SetNeuronValue(null, "Found", 1);
//yse, we have returned to a landmark we've been at before
currentLandmark = best;
currentEvent = currentLandmark.ReferencedBy[0].T;
//we need to reorient ourselves to face the same way as we did before (set a flag)
if (lastLandmark != currentLandmark)
{
lastLandmark = currentLandmark;
orienting = true;
}
}
else
{
//we're not near an existing landmark
currentLandmark = null;
currentEvent = null;
lastLandmark = null;
}
//this is on arrival back at a landmark
if (orienting)
{
Angle totalAngleError = GetTotalAngleError(near);
if (Abs(totalAngleError) > PI / 2 - .1f)
{
//turn in large increments until the angular error gets small
Angle angle = (float)PI / 2;
nmBehavior.TurnTo(angle);
return;
}
//this corrects for roundoff errors
CorrectModelPosition(nmModel, nmBehavior, best, near);
orienting = false;
return;
}
//we are in going-to-goal mode
if (currentTarget != null) //goingToGoal)
{
if (currentEvent != null) //currentEvent means we're at a decision point...check for the decision and execute it
{
Thing action = GoToGoal(currentTarget);
if (action != null)
{
float angle = GetAngleFromAction(action);
if (angle != 0)
{
nmBehavior.TurnTo(angle);
}
nmBehavior.MoveTo(1);
currentEvent = null;
return;
}
}
}
//we are in exploration mode
//We're not at a known landmark
//decide which way to turn at an obstacle
//If I am up to an obstacle...is there a decision or can I only go one way
float distAhead = nmModel.GetDistanceAtDirection(0);
float distLeft = nmModel.GetDistanceAtDirection((float)PI / 2);
float distRight = nmModel.GetDistanceAtDirection((float)-PI / 2);
bool canGoAhead = distAhead > 1;
bool canGoLeft = distLeft > 1;
bool canGoRight = distRight > 1;
int options = (canGoAhead ? 1 : 0) + (canGoRight ? 1 : 0) + (canGoLeft ? 1 : 0);
//First determine if there is a decision to be made or if there is only one option
if (options == 1 && auto)
{
//we have no choice but to follow the path
if (canGoAhead)
{
nmBehavior.MoveTo(1);
return;
}
if (canGoLeft)
{
nmBehavior.TurnTo((float)-PI / 2);
nmBehavior.MoveTo(1);
return;
}
if (canGoRight)
{
nmBehavior.TurnTo((float)PI / 2);
nmBehavior.MoveTo(1);
return;
}
}
else if (options == 0 && auto)
{
//we're trapped...note the color ahead and turn around
Thing thingAhead = nmModel.GetNearestThing();
currentTargetReached = thingAhead.References[2].T;
if (mostRecentDecisionPoint != null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentTargetReached);
mostRecentAction = null;
mostRecentDecisionPoint = null;
}
Neuron n1 = null;
if (currentTargetReached != null)
{
n1 = na.GetNeuronAt(currentTargetReached.Label);
}
//color a new goal neuron
if (n1 == null && currentTargetReached != null)
{
n1 = AddLabel(currentTargetReached.Label + " ");
n1.Model = Neuron.modelType.Color;
n1.SetValueInt((int)currentTargetReached.V);
na.GetNeuronLocation(n1, out int X, out int Y);
Neuron n2 = na.GetNeuronAt(X + 1, Y);
if (n2 != null)
{
n2.Label = currentTargetReached.Label;
}
}
if (currentTargetReached == currentTarget)
{
//if we're looking for a goal, we've reached it, so stop
//currentTarget = null;
currentEvent = null;
}
else
{
//make a U-Turn and start backtracking
nmBehavior.TurnTo((float)-PI);
}
}
else if (auto)
{
//we have a choice...
//if the current landmark is null, create a new landmark & Event
if (currentLandmark == null)
{
//Create new Landmark...it clones the points so they are not modified by the model module
Thing newLandmark = mEvent.CreateLandmark(near);
currentEvent = mEvent.CreateEvent(newLandmark);
currentLandmark = newLandmark;
if (mostRecentDecisionPoint != null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
}
}
else
{
if (mostRecentDecisionPoint != null && mostRecentDecisionPoint.Children.Find(t1 => t1.References[0].T == mostRecentAction) == null)
{
mEvent.AddOutcomePair(mostRecentDecisionPoint, mostRecentAction, currentEvent);
}
}
//TODO improve the method of finding something not tried before
//Decide which untried path to take
//priorities...1)continue ahead 2)left 3)right or randomized (depending on comment below)
Thing newAction = UKS.Labeled("NoAction");
List <Thing> possibleActions = new List <Thing>();
if (currentEvent.Children.Find(t => t.References[0].T.Label == "GoS") == null && canGoAhead)
{
possibleActions.Add(UKS.Labeled("GoS"));
}
if (currentEvent.Children.Find(t => t.References[0].T.Label == "LTurnS") == null && canGoLeft)
{
possibleActions.Add(UKS.Labeled("LTurnS"));
}
if (currentEvent.Children.Find(t => t.References[0].T.Label == "RTurnS") == null && canGoRight)
{
possibleActions.Add(UKS.Labeled("RTurnS"));
}
if (possibleActions.Count == 0 && currentEvent.Children.Find(t => t.References[0].T.Label == "UTurnS") == null)
{
newAction = UKS.Labeled("UTurnS");
}
else if (possibleActions.Count == 1)
{
newAction = possibleActions[0];
}
else if (possibleActions.Count > 0)
{
//for debugging, eliminate the ransomization by alternately commenting the 2 stmts below
// newAction = possibleActions[0];
newAction = possibleActions[rand.Next(possibleActions.Count)];
}
if (newAction.Label != "NoAction")
{
mostRecentAction = newAction;
mostRecentDecisionPoint = currentEvent;
Angle angle = GetAngleFromAction(newAction);
nmBehavior.TurnTo(angle);
nmBehavior.MoveTo(1);
}
else
{
//TODO: all actions at the current Event have been tried, is there another Event which hasn't been exhausted?
}
lastLandmark = currentLandmark;
return;
}
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
if (countDown > 0)
{
countDown--;
return;
}
//Input is an object in the model
//generate some alternate points of view (current position is the first)
//for each points of view
////can dest be seen?
////record distance
//if shortest distance > 0
///1) go to pov
///2) go to dest
///else recursive?
///
ModuleView naBehavior = MainWindow.theNeuronArray.FindModuleByLabel("ModuleBehavior");
if (naBehavior == null)
{
return;
}
ModuleBehavior nmBehavior = (ModuleBehavior)naBehavior.TheModule;
ModuleView naModel = theNeuronArray.FindModuleByLabel("Module2DModel");
Module2DModel nmModel = (Module2DModel)naModel.TheModule;
if (!nmBehavior.IsIdle())
{
return;
}
if (pvTry != null)
{
PointPlus pvTargetSave = new PointPlus {
P = pvTarget.P
};
pvTarget.X -= pvTry.X;
pvTarget.Y -= pvTry.Y;
pvTarget.Theta -= pvTry.Theta;
PointPlus pv1 = nmModel.CanISGoStraightTo(pvTarget, out Segment obstacle);
nmModel.ImagineEnd();
if (pv1 != null)
{
pvTry.Theta = -pvTry.Theta;
nmBehavior.TurnTo(pvTry.Theta);
nmBehavior.MoveTo(pvTry.R);
//SetNeuronValue("ModuleBehavior", "TurnTo", 1);
//SetNeuronValue("ModuleBehavior", "Theta", (float)pvTry.Theta);
//SetNeuronValue("ModuleBehavior", "MoveTo", 1);
//SetNeuronValue("ModuleBehavior", "R", (float)pvTry.R);
//SetNeuronValue("ModuleBehavior", "Done", 0);
//we made a partial move...update the target
pointsToTry.Clear();
tryAgain = true;
countDown = 5;
}
else
{
pvTarget = pvTargetSave;
}
pvTry = null;
return;
}
if (pointsToTry.Count > 0 && !nmModel.imagining)
{
pvTry = pointsToTry[0];
pointsToTry.RemoveAt(0);
nmModel.ImagineStart(pvTry, 0);
countDown = 15;
return;
}
if (GetNeuronValue(null, "Go") == 0 && !tryAgain)
{
return;
}
if (GetNeuronValue("ModuleBehavior", "Done") == 0)
{
return;
}
if (!tryAgain)
{
pvTarget = new PointPlus
{
R = GetNeuronValue(null, "R"),
Theta = GetNeuronValue(null, "Theta")
};
SetNeuronValue(null, "Go", 0);
SetNeuronValue(null, "R", 0);
SetNeuronValue(null, "Theta", 0);
}
// if (pvTarget.R == 0)
// pvTarget = nmModel.FindGreen().MidPoint();
if (pvTarget != null)
{
PointPlus pv1 = nmModel.CanISGoStraightTo(pvTarget, out Segment obstacle);
if (pv1 != null)
{
SetNeuronValue("ModuleBehavior", "TurnTo", 1);
SetNeuronValue("ModuleBehavior", "Theta", (float)-pv1.Theta);
SetNeuronValue("ModuleBehavior", "MoveTo", 1);
SetNeuronValue("ModuleBehavior", "R", (float)pv1.R);
tryAgain = false;
pvTry = null;
}
else
{
PointPlus pvTry1 = Utils.ExtendSegment(obstacle.P1.P, obstacle.P2.P, 0.5f, true);
PointPlus pvTry2 = Utils.ExtendSegment(obstacle.P1.P, obstacle.P2.P, 0.5f, false);
pointsToTry.Add(pvTry1);
pointsToTry.Add(pvTry2);
}
}
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
float[] dist = { .5f, .1f, 0, -.025f, -.1f };
float motionX = 0;
float motionY = 0;
if (na.Width < 3)
{
Initialize();
}
for (int i = 0; i < na.Height; i++)
{
if (na.GetNeuronAt(1, i).LastCharge > 0.9)
{
motionX = dist[i];
}
}
if (na.GetNeuronAt(1, 2).CurrentCharge != 0)
{
motionX = na.GetNeuronAt(1, 2).CurrentCharge;
na.GetNeuronAt(1, 2).SetValue(0);
}
if (na.GetNeuronAt(0, 2).LastCharge > 0.9)
{
motionY = 0.5f;
}
if (na.GetNeuronAt(2, 2).LastCharge > 0.9)
{
motionY = -0.5f;
}
//obsolete
ModuleRealityModel mrm = (ModuleRealityModel)FindModuleByType(typeof(ModuleRealityModel));
if (mrm != null)
{
mrm.Move(motionX);
}
Module3DSim m3D = (Module3DSim)FindModuleByType(typeof(Module3DSim));
if (m3D != null && motionX != 0)
{
m3D.Move(motionX);
}
bool moved = false;
Module2DSim m2D = (Module2DSim)FindModuleByType(typeof(Module2DSim));
if (m2D != null && motionX != 0 || motionY != 0)
{
moved = m2D.Move(motionX, motionY);
}
Module2DModel m2DModel = (Module2DModel)FindModuleByType(typeof(Module2DModel));
if (m2DModel != null && moved && motionX != 0 || motionY != 0)
{
m2DModel.Move(motionX, motionY);
}
Module2DVision m2DVision = (Module2DVision)FindModuleByType(typeof(Module2DVision));
if (m2DVision != null && motionX != 0)
{
m2DVision.ViewChanged();
}
}