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