private Thing MostLikelyPoint(PointPlus p1, ColorInt theColor)
{
Thing retVal = null;
if (UKSPoints == null)
{
return(null);
}
Angle closestTheta = Rad(180);
foreach (Thing t in UKSPoints)
{
if (t.ReferencedBy.Count > 0)
{
Segment s = SegmentFromUKSThing(t.ReferencedBy[0].T);
if (s == null)
{
continue;
}
if (s.theColor == theColor)
{
if (t.V is PointPlus p)
{
Angle deltaAngle = Abs(p.Theta - p1.Theta);
if (deltaAngle < closestTheta)
{
closestTheta = deltaAngle;
retVal = t;
}
}
}
}
}
return(retVal);
}
//this is asking can the entity directly see a target object or are there obstacles
public PointPlus CanISGoStraightTo(PointPlus midPoint, out Segment obstacle)
{
obstacle = null;
float closestDistance = 100;
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (UKS == null)
{
return(null);
}
bool ok = true;
foreach (Thing t in UKSSegments)
{
Segment s = SegmentFromUKSThing(t);
Utils.FindIntersection(new Point(0, 0), midPoint.P, s.P1.P, s.P2.P,
out bool lines_intersect, out bool segments_intersect,
out Point intersection, out Point close_p1, out Point closep2, out double collisionAngle);
if (segments_intersect)
{
ok = false;
Utils.FindDistanceToSegment(new Point(0, 0), s.P1.P, s.P1.P, out Point closest);
if (((Vector)closest).Length < closestDistance)
{
closestDistance = (float)((Vector)closest).Length;
obstacle = s;
}
}
}
if (ok)
{
return(midPoint);
}
return(null);
}
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);
}
}
//TODO add more properties to Stroke
public Thing AddStrokeToUKS(PointPlus P1, PointPlus P2)
{
ModuleUKS uks = (ModuleUKS)FindModuleByType(typeof(ModuleUKS));
if (uks is ModuleUKS UKS)
{
if (uks.Labeled("AbsStroke") == null)
{
uks.AddThing("AbsStroke", "Visual");
}
Thing t1 = uks.Valued(P1);
if (t1 == null)
{
t1 = UKS.AddThing("p" + pCount++, new Thing[] { UKS.Labeled("Point") }, P1);
}
Thing t2 = uks.Valued(P2);
if (t2 == null)
{
t2 = UKS.AddThing("p" + pCount++, new Thing[] { UKS.Labeled("Point") }, P2);
}
Thing newThing = UKS.AddThing("s" + strokeCount++, new Thing[] { UKS.Labeled("AbsStroke") }, null, new Thing[] { t1, t2 });
return(newThing);
}
return(null);
}
//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 FindBinocularBoundaries()
{
boundaries.Clear();
int start = 0;
for (int i = 0; i < LBoundaries.Count; i++)
{
for (int j = start; j < RBoundaries.Count; j++)
{
ColorInt cLL = LBoundaries[i].colorL;
ColorInt cLR = LBoundaries[i].colorR;
ColorInt cRL = RBoundaries[j].colorL;
ColorInt cRR = RBoundaries[j].colorR;
if (cLL == cRL || cLR == cRR)
{
//find distance and error
int l1 = LBoundaries[i].direction;
int r1 = RBoundaries[j].direction;
PointPlus leftPoint = FindDepth(l1, r1);
PointPlus leftPointError = FindDepth(l1 - 1, r1);
float error = leftPointError.R - leftPoint.R;
if (error < 0)
{
break;
}
leftPoint.Conf = error;
PointPlus pp = new PointPlus()
{
R = leftPoint.R, Theta = leftPoint.Theta, Conf = leftPoint.Conf
};
//if both sides of the point match...this creates two boundaries, one for each color
if (cLL == cRL)
{
BinocularBoundary bb = new BinocularBoundary()
{
p = leftPoint,
theColor = cLL,
changed = LBoundaries[i].changed,
};
boundaries.Add(bb);
}
if (cLR == cRR)
{
BinocularBoundary bb = new BinocularBoundary()
{
p = pp,
theColor = cLR,
changed = LBoundaries[i].changed,
};
boundaries.Add(bb);
}
start = j;
break;
}
}
}
}
void MoveTo(PointPlus target, float offset)
{
ModuleBehavior mBehavior = (ModuleBehavior)FindModleu(typeof(ModuleBehavior));
target.X -= offset; //body radius
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);
}
//maintain a list of objects in the current visual field
public void FireVisibleObjects()
{
ModuleView naUKS = theNeuronArray.FindAreaByLabel("Module2DUKS");
if (naUKS == null)
{
return;
}
ModuleUKSN UKS = (ModuleUKSN)naUKS.TheModule;
if (UKSSegments == null)
{
return;
}
//Thing tVisible = UKS.Labeled("Visible");
//clear all visiblility references
//for (int i = 0; i < UKSSegments.Count; i++)
// UKSSegments[i].RemoveReference(tVisible);
ModuleView naVision = theNeuronArray.FindAreaByLabel("Module2DVision");
if (naVision == null)
{
return;
}
int possibleViewAngles = naVision.Width;
float deltaTheta = Module2DVision.fieldOfView / possibleViewAngles;
for (int i = 0; i < possibleViewAngles; i++)
{
float theta = (float)-Module2DVision.fieldOfView / 2 + (i * deltaTheta);
PointPlus P = new PointPlus {
R = 10, Theta = theta
};
foreach (Thing t in UKSSegments)
{
Segment s = SegmentFromUKSThing(t);
if (s == null)
{
continue;
}
Utils.FindIntersection(new Point(0, 0), P.P, s.P1.P, s.P2.P,
out bool lines_intersect, out bool segments_intersect,
out Point intersection, out Point close_p1, out Point closep2, out double collisionAngle);
if (segments_intersect)
{
//TODO...only fire the closest at each point
UKS.Fire(t);
// t.AddReference(tVisible);
}
}
}
}
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
}
private PointPlus FindDepth(int l, int r)
{
//calculation using trig
Angle thetaA = GetDirectionFromNeuron(r - 0.5f, na.Width);
thetaA = PI / 2 - thetaA; //get angle to axis
Angle thetaB = GetDirectionFromNeuron(l - 0.5f, na.Width);
thetaB = PI / 2 - thetaB;
thetaB = PI - thetaB; //to get an inside angle
Angle thetaC = PI - thetaA - thetaB;
double A = Sin(thetaA) * (2 * eyeOffset) / Sin(thetaC);
thetaA -= PI / 2;
PointPlus P = new PointPlus {
Theta = (float)-thetaA, R = (float)A
}; //relative to left eye
P.Y -= eyeOffset; //correct to be centered between eyes
//alternate using vectors
PointPlus p1L = new PointPlus()
{
Y = -eyeOffset, X = 0
};
PointPlus p1R = new PointPlus()
{
Y = +eyeOffset, X = 0
};
Angle thetaL = (float)GetDirectionFromNeuron(r, na.Width);
PointPlus p2L = new PointPlus()
{
R = 20, Theta = thetaL
};
p2L.P = p2L.P + (Vector)p1L.P;
Angle thetaR = (float)GetDirectionFromNeuron(l, na.Width);
PointPlus p2R = new PointPlus()
{
R = 20, Theta = thetaR
};
p2R.P = p2R.P + (Vector)p1R.P;
Utils.FindIntersection(p1L.P, p2L.P, p1R.P, p2R.P, out bool lines_intersect, out bool segments_intersect, out Point intersection, out Point clost_p1, out Point close_p2, out double collisionAngle);
PointPlus retVal = new PointPlus()
{
P = intersection
};
return(retVal);
}
private bool InRange(PointPlus p, float rangeAhead)
{
bool retVal = false;
if (p.X <= 0)
{
return(retVal);
}
if (p.Y > -rangeAhead && p.Y < rangeAhead)
{
retVal = true;
}
return(retVal);
}
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 Explore(Segment s)
{
switch (state)
{
case 0: //go to midpoint
{
PointPlus target = s.MidPoint;
MoveTo(target, .2f);
doPush = 2;
doBackOff = true;
state++;
break;
}
case 1: //goto 1st turnpoint
{
Point p1 = s.MidPoint.P;
Point p2 = s.P1.P;
PointPlus target = new PointPlus {
X = (float)(p1.X + p2.X) / 2, Y = (float)(p1.Y + p2.Y) / 2
};
MoveTo(target, .2f);
doPush = 2;
doBackOff = true;
state++;
break;
}
case 2: //goto second turnpoint
{
Point p1 = s.MidPoint.P;
Point p2 = s.P2.P;
PointPlus target2 = new PointPlus {
X = (float)(p1.X + p2.X) / 2, Y = (float)(p1.Y + p2.Y) / 2
};
MoveTo(target2, .22f);
doPush = 2;
doBackOff = true;
state++;
break;
}
case 3: //face midpoint
{
FaceMidPoint(s);
state++;
break;
}
}
}
public void ImagineStart(PointPlus offset, float direction)
{
imagining = true;
if (imaginationOffset != null)
{
ImagineEnd();
}
imagining = true;
imaginationOffset = offset;
imaginationDirection = direction;
Rotate((float)-offset.Theta);
Move((float)offset.R);
Rotate((float)(direction - offset.Theta));
UpdateDialog();
}
public void ImagineEnd()
{
if (!imagining)
{
return;
}
ClearImagination();
Rotate((float)(-imaginationDirection + imaginationOffset.Theta));
Move((float)-imaginationOffset.R);
Rotate((float)imaginationOffset.Theta);
imaginationDirection = 0;
imaginationOffset = null;
imagining = false;
UpdateDialog();
}
private static void MovePhysObject(physObject ph, Point closest, PointPlus motion)
{
ph.P1 = ph.P1 + motion.V;
ph.P2 = ph.P2 + motion.V;
//handle rotation if object not hit in the middle
//we know that our point "closest" moves with "motion"...rotation should be about "closest"
Segment s = new Segment
{
P1 = new PointPlus()
{
P = ph.P1
},
P2 = new PointPlus()
{
P = ph.P2
}
};
PointPlus contactPoint = new PointPlus()
{
P = closest
};
PointPlus offset = new PointPlus()
{
P = (Point)(contactPoint.V - s.MidPoint.V)
};
double cross = Vector.CrossProduct(offset.V, motion.V);
float rotation = 10 * Sign(cross);
float rotationRatio = (float)(offset.V.Length / s.Length * 2);
PointPlus V1 = new PointPlus()
{
P = (Point)(s.P1.P - contactPoint.P)
};
PointPlus V2 = new PointPlus()
{
P = (Point)(s.P2.P - contactPoint.P)
};
V1.Theta += Rad(rotation * rotationRatio);
V2.Theta += Rad(rotation * rotationRatio);
ph.P1 = (Point)V1.V + contactPoint.V;
ph.P2 = (Point)V2.V + contactPoint.V;
}
public void UpdateEndpointFromVision(PointPlus P1, ColorInt theColor, bool moved)
{
//Debug.WriteLine("UpdatePoint: " + P1 + theColor);
Thing match = MostLikelyPoint(P1, theColor);
if (match != null)
{
if (match.V is PointPlus p)
{
if (P1.Conf < p.Conf || moved)
{
match.V = P1;
}
}
UpdateDialog();
}
}
public static void OrderSegment(object x)
{
if (x is Thing t)
{
if (((PointPlus)t.References[0].T.V).Theta > ((PointPlus)t.References[1].T.V).Theta)
{
Link temp = t.References[0]; t.References[0] = t.References[1]; t.References[1] = temp;
}
}
if (x is Segment s)
{
if (s.P1.Theta > s.P2.Theta)
{
PointPlus temp = s.P1; s.P1 = s.P2; s.P2 = temp;
}
}
}
//not used but may be useful elsewhere.
Segment NextPosition(Segment s, Motion m)
{
PointPlus oldMidPoint = s.MidPoint;
PointPlus newMidPoint = oldMidPoint + m;
PointPlus dP1 = s.P1 - oldMidPoint;
PointPlus dP2 = s.P2 - oldMidPoint;
dP1.Theta += m.rotation;
dP2.Theta += m.rotation;
Segment retVal = new Segment()
{
P1 = newMidPoint + dP1,
P2 = newMidPoint + dP2,
theColor = s.theColor,
};
return(retVal);
}
//touch is the intersection of an arm with an obstacle
public void HandleTouch()
{
//arm[0] is left [1] is right
armActual = new Point[armRelative.Length];
//is there an object intersecting the arm?
for (int i = 0; i < armRelative.Length; i++)
{
PointPlus pv = new PointPlus {
P = (Point)armRelative[i]
};
pv.Theta = entityDirection1 + pv.Theta;
Point armPositionAbs = entityPosition + (Vector)pv.P;
HandleTouch(armPositionAbs, i);
}
motion = new PointPlus()
{
R = 0, Theta = 0
};
}
private void CreateCenteredStroke(Segment0 s1, Segment0 s2)
{
PointPlus p1 = new PointPlus(s1.p1);
PointPlus p2 = new PointPlus(s1.p2);
PointPlus p3 = new PointPlus(s2.p1);
PointPlus p4 = new PointPlus(s2.p2);
//transfer first point to origin
PointPlus p10 = p1 - p1;
PointPlus p20 = p2 - p1;
PointPlus p30 = p3 - p1;
PointPlus p40 = p4 - p1;
//rotate so both segments are horizontal.
Angle a = p20.Theta;
PointPlus p10r = p10; p10r.Theta -= a;
PointPlus p20r = p20; p20r.Theta -= a;
PointPlus p30r = p30; p30r.Theta -= a;
PointPlus p40r = p40; p40r.Theta -= a;
double minX = Min(new double[] { p10r.X, p20r.X, p30r.X, p40r.X });
double maxX = Max(new double[] { p10r.X, p20r.X, p30r.X, p40r.X });
double y = (p30r.Y + p40r.Y) / 2;
PointPlus pa = new PointPlus((float)minX, (float)y / 2);
PointPlus pb = new PointPlus((float)maxX, (float)y / 2);
pa.Theta += a;
pb.Theta += a;
pa = pa + p1;
pb = pb + p1;
Stroke theStroke = new Stroke
{
width = (float)y,
p1 = pa.P,
p2 = pb.P,
};
strokes.Add(theStroke);
}
private void TheCanvas_MouseRightButtonDown(object sender, MouseButtonEventArgs e)
{
Module2DSim parent = (Module2DSim)base.ParentModule;
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;
Point position = e.GetPosition(theCanvas);
PointPlus v = new PointPlus {
P = (Point)(position - parent.entityPosition)
};
float dist = (float)v.R;
double angle = (float)v.Theta;
double deltaAngle = angle - parent.entityDirection1;
ModuleView naGoToDest = MainWindow.theNeuronArray.FindAreaByLabel("ModuleGoToDest");
if (naGoToDest != null)
{
naGoToDest.GetNeuronAt("Go").SetValue(1);
naGoToDest.GetNeuronAt("Theta").SetValue((float)deltaAngle);
naGoToDest.GetNeuronAt("R").SetValue(dist);
}
else
{
ModuleView naBehavior = MainWindow.theNeuronArray.FindAreaByLabel("ModuleBehavior");
if (naBehavior != null)
{
naBehavior.GetNeuronAt("TurnTo").SetValue(1);
naBehavior.GetNeuronAt("Theta").SetValue((float)-deltaAngle);
naBehavior.GetNeuronAt("MoveTo").SetValue(1);
naBehavior.GetNeuronAt("R").SetValue(dist);
}
}
}
public Thing AddSegmentToUKS(PointPlus P1, PointPlus P2, int theColor, PointPlus motion = null, bool addToModel = true)
{
ModuleUKS nmUKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (nmUKS is ModuleUKS UKS)
{
Thing t1, t2;
Thing t3 = null;
t1 = UKS.AddThing("p" + pCount++, new Thing[] { UKS.Labeled("Point") }, P1);
t2 = UKS.AddThing("p" + pCount++, new Thing[] { UKS.Labeled("Point") }, P2);
if (addToModel)
{
t1.AddParent(UKS.Labeled("ModelThing"));
t2.AddParent(UKS.Labeled("ModelThing"));
}
if (motion != null)
{
t3 = UKS.AddThing("m" + mCount++, new Thing[] { UKS.Labeled("Motion") }, motion);
}
Thing color = UKS.Valued(theColor);
if (color == null)
{
color = UKS.AddThing("c" + cCount++, new Thing[] { UKS.Labeled("Color") }, theColor);
}
Thing newThing = null;
if (motion != null)
{
newThing = UKS.AddThing("s" + sCount++, new Thing[] { UKS.Labeled("Segment") }, null, new Thing[] { t1, t2, color, t3 });
}
else
{
newThing = UKS.AddThing("s" + sCount++, new Thing[] { UKS.Labeled("Segment") }, null, new Thing[] { t1, t2, color });
}
return(newThing);
}
return(null);
}
List <PointPlus> GetListFromShape(Thing theShape)
{
List <PointPlus> retVal = new List <PointPlus>();
if (!theShape.Label.Contains("Shape"))
{
return(retVal);
}
if (theShape.Children.Count == 0)
{
return(retVal);
}
Thing firstCorner = theShape.Children[0];
Thing curCorner = theShape.Children[0];
do
{ //TODO what if the references are out of order?
Thing theAngle = curCorner.References[0].T;
Link l = curCorner.References[1];
if (l is Relationship r)
{
Thing theLength = r.relationshipType;
float.TryParse(theAngle.Label.Substring(3), out float theAngle1);
float.TryParse(theLength.Label.Substring(3), out float theLength1);
PointPlus pp = new PointPlus {
R = theLength1, Theta = Angle.FromDegrees(theAngle1)
};
retVal.Add(pp);
}
curCorner = curCorner.References[1].T;
} while (curCorner != firstCorner);
return(retVal);
}
public Thing GetNearestThing(Angle theta, out float dist)
{
Thing nearest = null;
dist = float.MaxValue;
Segment s = null;
foreach (Thing t in UKSSegments)
{
s = SegmentFromUKSThing(t);
//OrderSegment(s);
//does this object cross the given visual angle?
PointPlus pv = new PointPlus {
R = 10, Theta = theta
};
Utils.FindIntersection(new Point(-.2, 0), pv.P, s.P1.P, s.P2.P,
out bool lines_intersect, out bool segments_intersect, out Point intersection, out Point clos_p1, out Point close_p2, out double collisionAngle);
if (!segments_intersect)
{
continue;
}
//and is it the nearest?
Vector v = (Vector)intersection;
if (v.Length < dist)
{
nearest = t;
dist = (float)v.Length;
}
}
if (nearest != null)
{
return(nearest);
}
return(null);
}
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);
}
}
//TODO: rewrite again
public Thing AddSegmentFromVision(PointPlus P1, PointPlus P2, ColorInt theColor, bool moved)
{
Thing retVal = null;
Debug.WriteLine("AddSegment: " + P1 + P2 + theColor);
//determine if the segment is already in the UKS.
//Correct it if it is there, add it if it is not.
//FUTURE: detect motion
if (theColor == 0)
{
return(null);
}
Segment newSegment = new Segment()
{
P1 = P1, P2 = P2, theColor = theColor
};
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
GetSegmentsFromUKS();
if (UKS != null)
{
//it's easier if we sort by theta
OrderSegment(newSegment);
retVal = MostLikelySegment(newSegment);
if (retVal != null)
{
//UKS.Fire(match);
//OrderSegment(match);
//Segment s = SegmentFromUKSThing(match);
//float newVisualWidth = newSegment.VisualWidth();
//float matchVisualWidth = s.VisualWidth();
////if the newVisualWidth is bigger, an adjustment is needed
////this happens if the initial view was occluded but now it is less
//Thing match1 = MostLikelyPoint(newSegment.P1, newSegment.theColor);
//Thing match2 = MostLikelyPoint(newSegment.P2, newSegment.theColor);
//if (match1 != null && match2 != null)
//{
// if (newSegment.P1.Conf < s.P1.Conf)
// {
// s.P1.Conf = newSegment.P1.Conf;
// s.P1.R = newSegment.P1.R;
// s.P1.Theta = newSegment.P1.Theta;
// }
// if (newSegment.P2.Conf < s
// .P2.Conf)
// {
// s.P2.Conf = newSegment.P2.Conf;
// s.P2.R = newSegment.P2.R;
// s.P2.Theta = newSegment.P2.Theta;
// }
//}
////there is a significant point mismatch...
//else
//{
// if (match1 == null && newSegment.P1.R > s.P1.R)
// {
// s.P1.Conf = newSegment.P1.Conf;
// s.P1.R = newSegment.P1.R;
// s.P1.Theta = newSegment.P1.Theta;
// }
// if (match2 == null && newSegment.P2.R > s.P2.R)
// {
// s.P2.Conf = newSegment.P2.Conf;
// s.P2.R = newSegment.P2.R;
// s.P2.Theta = newSegment.P2.Theta;
// }
//}
}
else
{
retVal = AddSegmentToUKS(P1, P2, theColor);
UKS.Fire(retVal);
}
UpdateDialog();
}
return(retVal);
}
//get input from touch... accurate locations, no color
public bool AddSegmentFromTouch(PointPlus P1, PointPlus P2, PointPlus motion, int arm)
{
//if conf=0, it's a known endpoint. conf=1, not an endpoint
ModuleUKSN UKS = (ModuleUKSN)FindModuleByType(typeof(ModuleUKSN));
if (UKS is null)
{
return(false);
}
if (UKSSegments is null)
{
return(false);
}
if (imagining)
{
return(false);
}
ColorInt theColor = Utils.ColorToInt(Colors.Wheat);
Segment newSegment = new Segment()
{
P1 = P1, P2 = P2, theColor = theColor
};
//OrderSegment(newSegment);
Thing t1 = GetNearestThing(newSegment.MidPoint.Theta, out float dist1);
//TODO: for motion testing
t1 = UKS.Labeled("s0");
if (t1 == null)
{
//TODO: merge mutliple segments
// AddSegmentToUKS(P1, P2, theColor, motion); //don't store motion with the segment (yet)
AddSegmentToUKS(P1, P2, theColor);
}
else if (dist1 < 1)
{
Segment prevSegment = SegmentFromUKSThing(t1);
PointPlus prevMidpoint = prevSegment.MidPoint;
Angle oldM = prevSegment.Angle;
Angle newM = newSegment.Angle;
PointPlus offset = new PointPlus()
{
R = prevSegment.Length, Theta = newM
};
if (P1.Conf == 0 && P2.Conf == 0) //we're given both endpoints
{
prevSegment.P1.P = P1.P; prevSegment.P1.Conf = 0;
prevSegment.P2.P = P2.P; prevSegment.P2.Conf = 0;
}
else if (P1.Conf == 0)
{
prevSegment.P1.P = P1.P; prevSegment.P1.Conf = 0;
prevSegment.P2.P = P1.P - offset.V;
}
else if (P2.Conf == 0)
{
prevSegment.P1.P = P2.P; prevSegment.P2.Conf = 0;
prevSegment.P2.P = P2.P + offset.V;
}
else
{
//we're not near an endpoint--match the modpoint as close as possible & preserve length
//make the segment match the two points
PointPlus newMidpoint1 = new PointPlus()
{
P = (Point)GetClosestPointOnLine(P1.V, P2.V, prevMidpoint.V),
};
//offset is the dietance from the midpoint to each end
offset.R = offset.R / 2;
PointPlus newP1 = new PointPlus()
{
P = newMidpoint1.P + offset.V
};
PointPlus newP2 = new PointPlus()
{
P = newMidpoint1.P - offset.V
};
prevSegment.P1.R = newP1.R; prevSegment.P1.Theta = newP1.Theta;
prevSegment.P2.R = newP2.R; prevSegment.P2.Theta = newP2.Theta;
}
PointPlus newMidpoint = prevSegment.MidPoint;
newMidpoint.P = newMidpoint.P - prevMidpoint.V;
if (newMidpoint.R > 0.01 && motion.R != 0)
{
if (prevSegment.Motion == null)
{
prevSegment.Motion = new PointPlus();
Thing tMotion = UKS.AddThing("m" + mCount++, UKS.Labeled("Point"));
tMotion.V = prevSegment.Motion;
t1.AddReference(tMotion);
}
prevSegment.Motion.R = motion.R;
prevSegment.Motion.Theta = motion.Theta;
prevSegment.Motion.Conf = newM - oldM;
}
}
UpdateDialog();
return(false);
}
