public Module3DSimDlg()
{
InitializeComponent();
Random rand = new Random();
for (int i = 0; i < 20; i++)
{
System.Windows.Media.Color tempColor = Colors.Red;
double x = rand.NextDouble();
if (x > .33)
{
tempColor = Colors.Blue;
}
if (x > .67)
{
tempColor = Colors.Green;
}
physObject newObject = new physObject
{
thePos = new Point3D(rand.NextDouble() * 10 - 5, 0, rand.NextDouble() * 10 - 5),
theColor = tempColor,
scaleX = .5f,
scaleY = .75f
};
objects.Add(newObject);
}
}
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 override void Initialize()
{
TrainingSamples = new string[]
{
"what color is this:Say:Positive",
"what is this:Say:Positive",
"go:Go:Positive",
"stop:Stop:Positive",
//"sallie:Attn:Positive",
//"turn around:UTurn:Positive",
"turn left:LTurn:Positive",
"turn right:RTurn:Positive",
"This is [color]:NoAction:Positive",
"[color]:NoAction:Positive"
};
entityTrack.Clear();
entityPosition = new Point(0, 0);
entityTrack.Add(entityPosition);
entityDirection1 = 0;
na.GetNeuronAt(0).Label = "Actions";
na.GetNeuronAt(1).Label = "Colors";
na.GetNeuronAt(2).Label = "Train";
armRelative = new Vector[] { new Vector(.4, .4), new Vector(.4, -.4) };
armActual = new Point[2];
for (int i = 0; i < armRelative.Length; i++)
{
armActual[i] = entityPosition + armRelative[i];
}
objects.Clear();
//build a pen to keep the entity inside
objects.Add(new physObject()
{
P1 = new Point(boundarySize, boundarySize), P2 = new Point(boundarySize, -boundarySize), theColor = Colors.Black, isMobile = false
});
objects.Add(new physObject()
{
P1 = new Point(boundarySize, -boundarySize), P2 = new Point(-boundarySize, -boundarySize), theColor = Colors.Black, isMobile = false
});
objects.Add(new physObject()
{
P1 = new Point(-boundarySize, -boundarySize), P2 = new Point(-boundarySize, boundarySize), theColor = Colors.Black, isMobile = false
});
objects.Add(new physObject()
{
P1 = new Point(-boundarySize, boundarySize), P2 = new Point(boundarySize, boundarySize), theColor = Colors.Black, isMobile = false
});
int colorCount = 0;
//Color[] theColors = new Color[] {
// Colors.Red,Colors.Blue,Colors.Orange,Colors.Magenta,Colors.Pink,
// Colors.Lime,Colors.MediumAquamarine,Colors.LightBlue,Colors.Yellow,Colors.PeachPuff,
// Colors.GreenYellow,Colors.Cyan,Colors.DarkBlue,Colors.DarkGreen,Colors.LawnGreen,
// Colors.BlueViolet,Colors.DarkRed,Colors.DarkSeaGreen,Colors.LightCoral,Colors.Lavender,Colors.DarkOrange};
Color currentColor = Colors.Blue;
List <Color> theColors = new List <Color>();
theColors.Add(Colors.Red);
theColors.Add(Colors.Lime);
theColors.Add(Colors.Blue);
theColors.Add(Colors.Magenta);
theColors.Add(Colors.Cyan);
theColors.Add(Colors.Orange);
theColors.Add(Colors.Purple);
theColors.Add(Colors.Maroon);
theColors.Add(Colors.Green);
theColors.Add(Colors.Crimson);
PropertyInfo[] p1 = typeof(Colors).GetProperties();
int count = 0;
foreach (PropertyInfo p in p1)
{
if (count++ > 7)
{
Color c = (Color)p.GetValue(null);
if (c != Colors.White && c != Colors.Black && c != Colors.AliceBlue && c != Colors.GhostWhite && c != Colors.Honeydew &&
c != Colors.Azure && c != Colors.Beige && c != Colors.Bisque && c != Colors.Cornsilk && c != Colors.AntiqueWhite &&
c != Colors.Cyan && c != Colors.FloralWhite && c != Colors.Gray && c != Colors.Cyan)
{
if (!theColors.Contains(c))
{
theColors.Add(c);
}
}
}
}
foreach (Neuron n in na.Neurons())
{
na.GetNeuronLocation(n, out int x1, out int y1);
TransformPoint(ref x1, ref y1);
foreach (Synapse s in n.Synapses)
{
if (s.TargetNeuron != n.Id)// && s.Weight != 10)
{
na.GetNeuronLocation(s.TargetNeuron, out int x2, out int y2);
TransformPoint(ref x2, ref y2);
physObject newObject = new physObject
{
P1 = new Point(x1 - 0.5, y1 - 0.5),
P2 = new Point(x2 - 0.5, y2 - 0.5),
theColor = theColors[colorCount],
Aroma = -1,
Temperature = 10,
isMobile = (s.Weight == 1) ? true : false,
};
if (s.Weight < 1)
{
newObject.motion = new Vector(s.Weight - .5f, 0);
}
if (s.Weight < 0)
{
newObject.motion = new Vector(0, -.5f - s.Weight);
}
objects.Add(newObject);
colorCount = (colorCount + 1) % theColors.Count;
int currentColorInt = Utils.ColorToInt(currentColor);
currentColorInt--;
currentColor = Utils.IntToColor(currentColorInt);
}
}
}
HandleVision();
UpdateDialog();
}
private void HandleTouch(Point armPositionAbs, int index)
{
armActual[index] = armPositionAbs;
SetNeuronValue("Module2DTouch", 0, index, 0);
SetNeuronValue("Module2DTouch", 8, index, 2);
//this all works in absolute coordinates
for (int i = 0; i < objects.Count; i++)
{
Point P1 = objects[i].P1;
Point P2 = objects[i].P2;
physObject ph = objects[i];
Utils.FindIntersection(P1, P2, entityPosition, armPositionAbs, out bool linesintersect, out bool segments_intersect,
out Point Intersection, out Point close_p1, out Point close_p2, out double collisionAngle);
if (segments_intersect)
{//we're touching
//can we feel the end of the segment?
float p1IsEndpt = 0;
float p2IsEndpt = 0;
float assumedLength = 0.5f;
float l1 = assumedLength; //initial (guess) length of segment
float l2 = assumedLength;
Vector dist1 = P1 - Intersection;
if (dist1.Length <= assumedLength)
{
p1IsEndpt = 1;
l1 = (float)dist1.Length;
}
Vector dist2 = P2 - Intersection;
if (dist2.Length <= assumedLength)
{
p2IsEndpt = 1;
l2 = (float)dist2.Length;
}
PointPlus armPositionRel = new PointPlus {
P = (Point)(Intersection - entityPosition)
};
armPositionRel.Theta = armPositionRel.Theta - entityDirection1;
float[] neuronValues = new float[12];
//everything from here out is coordinates relative to self
//neurons: 0:touch 1:armAngle 2:armDistance 3: sensedLineAngle 4: conf1 5: len1 6: conf2 7: len2 8: Release
neuronValues[0] = 1;
neuronValues[1] = armPositionRel.R;
neuronValues[2] = armPositionRel.Theta;
neuronValues[3] = (float)collisionAngle;
neuronValues[4] = (float)p1IsEndpt;
neuronValues[5] = (float)l1;
neuronValues[6] = (float)p2IsEndpt;
neuronValues[7] = (float)l2;
neuronValues[8] = 0;
neuronValues[9] = motion.R;
neuronValues[10] = motion.Theta;
neuronValues[11] = motion.Conf;
SetNeuronVector("Module2DTouch", true, index, neuronValues);
if (motion.R != 0)
{
motion.R = motion.R;
}
armActual[index] = Intersection;
break;
}
}
}
//a collision is the intersection of the desired newPosition and an obstacle
private bool CheckForCollisions(Point newPosition)
{
motion = new PointPlus()
{
R = 0, Theta = 0
};
bool retVal = false;
for (int i = 0; i < objects.Count; i++)
{
Point P1 = objects[i].P1;
Point P2 = objects[i].P2;
physObject ph = objects[i];
double dist = Utils.FindDistanceToSegment(newPosition, ph.P1, ph.P2, out Point closest);
if (dist < bodyRadius)
{
PointPlus collPt = new PointPlus {
P = (Point)(closest - newPosition)
};
if (!objects[i].isMobile) //collision
{
SetNeuronValue("ModuleBehavior", "Coll", 1);
collPt.Theta -= entityDirection1;
SetNeuronValue("ModuleBehavior", "CollAngle", collPt.Theta);
retVal = true;;
}
else //move the object
{
float distToMoveObject = bodyRadius - (float)dist;
motion = new PointPlus()
{
P = collPt.P
};
motion.R = distToMoveObject;
Point oldPoint1 = new Point(ph.P1.X, ph.P1.Y);
Point oldPoint2 = new Point(ph.P2.X, ph.P2.Y);
Segment s = new Segment();
s.P1 = new PointPlus()
{
P = ph.P1
};
s.P2 = new PointPlus()
{
P = ph.P2
};
Angle oldM = s.Angle;
MovePhysObject(ph, closest, motion);
s.P1 = new PointPlus()
{
P = ph.P1
};
s.P2 = new PointPlus()
{
P = ph.P2
};
Angle newM = s.Angle;
motion.Conf = newM - oldM;
//TODO check for collisions with this object and other objects
for (int j = 0; j < objects.Count; j++)
{
if (j == i)
{
continue;
}
physObject ph2 = objects[j];
if (!ph2.isMobile)
{
continue;
}
FindIntersection(ph.P1, ph.P2, ph2.P1, ph2.P2, out bool lines_intersect, out bool segments_intersect,
out Point intersection, out Point close_p1, out Point close_p2, out double collisionAngle);
if (segments_intersect)
{
PointPlus endMotion = new PointPlus();
float dist1 = (float)((Vector)(intersection - (Vector)ph.P1)).Length;
float dist2 = (float)((Vector)(intersection - (Vector)ph.P2)).Length;
if (dist1 < dist2)
{
endMotion.P = ph.P1 - (Vector)oldPoint1;
}
else
{
endMotion.P = ph.P2 - (Vector)oldPoint2;
}
MovePhysObject(objects[j], intersection, endMotion);
}
}
}
}
}
return(retVal);
}
