void MoveTo(PointPlus target, float offset)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModuleByType(typeof(ModuleBehavior));
target.X -= offset; //body radius
mBehavior.TurnTo(-target.Theta);
mBehavior.MoveTo(target.R);
mBehavior.TurnTo(target.Theta); //return to previous orientation
}
void DoBackOff(Segment s)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
PointPlus target = new PointPlus(s.MidPoint + new PointPlus {
R = 2f, Theta = s.Angle - PI / 2
});
mBehavior.TurnTo(-target.Theta);
mBehavior.MoveTo(target.R);
mBehavior.TurnTo(target.Theta); //return to previous orientation
}
void DoFaceSegment(Segment s)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
Utils.FindDistanceToSegment(s, out Point closest);
PointPlus closestPP = new PointPlus {
P = closest
};
mBehavior.TurnTo(-closestPP.Theta);
}
void Push(Segment s)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
float dist = (float)Utils.FindDistanceToSegment(s, out Point closest);
PointPlus closestPP = new PointPlus {
P = closest
};
mBehavior.TurnTo(-closestPP.Theta);
mBehavior.MoveTo(closestPP.R - .1f);
}
public override void Fire()
{
Init(); //be sure to leave this here to enable use of the na variable
if (UKSPoints == null || UKSPoints.Count == 0)
{
GetSegmentsFromUKS();
}
ModuleBehavior nmBehavior = (ModuleBehavior)FindModuleByType(typeof(ModuleBehavior));
Thing obstacle = NearestThingAhead();
if (obstacle != null)
{
Segment s = SegmentFromUKSThing(obstacle);
Utils.FindDistanceToSegment(new Point(0, 0), s.P1.P, s.P2.P, out Point closest);
double dist = ((Vector)closest).Length;
if (dist < 0.6f)
{
//we are approaching an obsctacle.
SetNeuronValue(null, "Obstacle", 1);
}
}
}
void FaceMidPoint(Segment s)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
mBehavior.TurnTo(-s.MidPoint.Theta);
}
void GoToGoal(Segment sCurrent, Segment sTarget)
{
//instead of working with endpoints, we'll just work with midpoint and rotation because the length can't change
Seg pCurrent = new Seg {
P = sCurrent.MidPoint.P, rotation = sCurrent.Angle
};
Seg pTarget = new Seg {
P = sTarget.MidPoint.P, rotation = sTarget.Angle
};
Motion neededMotion = DistanceFromTarget(pTarget, pCurrent);
//there are two exit cases...here we test for absolute closeness, below we test for the best action makes thing further
if (Abs(neededMotion.rotation) < .05 && Abs(neededMotion.X) < .1 && Abs(neededMotion.Y) < .1)
{
endTarget = null;
}
else
{
ModuleUKSN UKS = (ModuleUKSN)FindModleu(typeof(ModuleUKSN));
Thing bestEvent = null;
if (neededMotion.R > .2f)
{
//create a temp distination which is slightly offset
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
Seg tempTarget = new Seg()
{
P = sTarget.MidPoint.P + new PointPlus {
R = .15f, Theta = sTarget.Angle - PI / 2
}.V, rotation = 0
};
neededMotion = DistanceFromTarget(tempTarget, pCurrent);
bestEvent = UKS.Labeled("E0");
if (neededMotion.Theta < -.05)
{
bestEvent = UKS.Labeled("E2");
}
if (neededMotion.Theta > .05)
{
bestEvent = UKS.Labeled("E1");
}
}
else if (neededMotion.R > .01)
{
if (neededMotion.rotation < -.02)
{
bestEvent = UKS.Labeled("E2");
}
else
{
bestEvent = UKS.Labeled("E1");
}
}
if (bestEvent != null)
{
Motion m = (Motion)bestEvent.Children[0].References[1].T.V;
Seg nextPosition = NextPosition(pCurrent, (Motion)bestEvent.Children[0].References[1].T.V);
Motion next = DistanceFromTarget(pTarget, nextPosition);
//if (next.R < neededMotion.R || Abs(next.rotation) > Utils.Rad(5))
{
GoToLandmark(bestEvent.References[0].T, sCurrent);
doPush = 2;
doBackOff = true;
}
//// else
// {
// endTarget = null;
// }
}
//find the event with the most desireable outcome and then go the the landmark.
//Thing bestEvent = null;
//List<Thing> events = UKS.Labeled("Event").Children;
//Seg[] distances = new Seg[events.Count];
////first time through loop initialize array and take first action
//for (int i = 0; i < events.Count; i++)
//{
// distances[i] = new Seg() { P = neededMotion.P, rotation = neededMotion.rotation };
// Thing tEvent = events[i];
// Motion motion = (Motion)tEvent.Children[0].References[1].T.V;
// distances[i] = NextPosition(distances[i], motion);
//}
////second time through loop, add to array
//for (int j = 0; j < 3; j++) //three steps
//{
// for (int i = 0; i < events.Count; i++)
// {
// for (int k = 0; k < events.Count; k++)
// {
// Thing tEvent = events[k];
// Motion motion = (Motion)tEvent.Children[0].References[1].T.V;
// Seg newS = NextPosition(distances[i], motion);
// if (newS.Val < distances[i].Val)
// { distances[i] = newS; }
// }
// }
//}
//float bestDist = float.MaxValue;
//for (int i = 0; i < events.Count; i++) //last time through loop, find best
//{
// if (distances[i].Val < bestDist)
// {
// bestDist = distances[i].Val;
// bestEvent = events[i];
// }
//}
//Motion motion1 = (Motion)bestEvent.Children[0].References[1].T.V;
//Seg newS1 = NextPosition(pCurrent, motion1);
//if (newS1.Val < pTarget.Val)
//{
// GoToLandmark(bestEvent.References[0].T, sCurrent);
// doPush = 2;
// doBackOff = true;
//}
//else //our best move makes things worse
//{
// endTarget = null;
//}
}
}
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 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);
}
}
}
