123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574 |
- using System;
- using System.Collections.Generic;
- using System.IO;
- using System.Xml.Serialization;
- using OpenTK.Graphics.OpenGL;
- using Raahn;
- namespace RaahnSimulation
- {
- [XmlRoot("Robot")]
- public class CarConfig
- {
- [XmlElement("X")]
- public double x;
- [XmlElement("Y")]
- public double y;
- [XmlElement("Angle")]
- public double angle;
- }
- public partial class Car : Entity
- {
- public const double HALF_QUERY_WIDTH = Simulator.WORLD_WINDOW_WIDTH / 2.0;
- public const double HALF_QUERY_HEIGHT = Simulator.WORLD_WINDOW_HEIGHT / 2.0;
- private const int UNIQUE_GROUP_TYPES = 3;
- private const double CONTROL_THRESHOLD = 0.5;
- private const double ROTATE_SPEED = 2.0;
- private const double SPEED_X = 15.0;
- private const double SPEED_Y = 12.0;
- private const double MAX_ROTATE = 1.0;
- private const double MIN_ROTATE = 0.0;
- //Between 0 and twice ROTATE_SPEED
- private const double ROTATE_RANGE = 2.0 * ROTATE_SPEED;
- public List<Entity> entitiesHovering;
- private bool configLoaded;
- private bool userControlled;
- private uint rangeFinderCount;
- private uint pieSliceSensorCount;
- private QuadTree quadTree;
- private ControlScheme.SchemeFunction controlScheme;
- //Each list corresponds to a modulation scheme.
- private List<List<uint>> modulationSignals;
- private List<List<int>> orderdGroupIds;
- private List<ModulationScheme.SchemeFunction> modulationSchemes;
- private List<RangeFinderGroup> rangeFinderGroups;
- private List<PieSliceSensorGroup> pieSliceSensorGroups;
- private NeuralNetwork brain;
- public Car(Simulator sim, QuadTree tree) : base(sim)
- {
- texture = TextureManager.TextureType.CAR;
- type = EntityType.CAR;
- userControlled = false;
- quadTree = tree;
- modulationSignals = new List<List<uint>>();
- modulationSchemes = new List<ModulationScheme.SchemeFunction>();
- rangeFinderGroups = new List<RangeFinderGroup>();
- pieSliceSensorGroups = new List<PieSliceSensorGroup>();
- speed.x = SPEED_X;
- speed.y = SPEED_Y;
- entitiesHovering = new List<Entity>();
- //3 types of ids, input, hidden, output
- orderdGroupIds = new List<List<int>>(UNIQUE_GROUP_TYPES);
- for (int i = 0; i < UNIQUE_GROUP_TYPES; i++)
- orderdGroupIds.Add(new List<int>());
- }
- //Updates only sensors.
- public void UpdateMinimal()
- {
- base.Update();
- for (int i = 0; i < rangeFinderGroups.Count; i++)
- rangeFinderGroups[i].Update();
- for (int i = 0; i < pieSliceSensorGroups.Count; i++)
- pieSliceSensorGroups[i].Update();
- }
- public void Control()
- {
- if (controlScheme != null)
- userControlled = controlScheme(this);
- }
- public override void Update()
- {
- base.Update();
- double worldX = GetWorldX();
- double worldY = GetWorldY();
- Utils.Vector2 lowerLeft = camera.TransformWorld(worldX - HALF_QUERY_WIDTH, worldY - HALF_QUERY_HEIGHT);
- Utils.Vector2 upperRight = camera.TransformWorld(worldX + HALF_QUERY_WIDTH, worldY + HALF_QUERY_HEIGHT);
- AABB viewBounds = new AABB(upperRight.x - lowerLeft.x, upperRight.y - lowerLeft.y);
- viewBounds.Translate(lowerLeft.x, lowerLeft.y);
- Utils.LineSegment collisionLine = new Utils.LineSegment();
- Utils.Point2 original = new Utils.Point2(center.x, center.y);
- Utils.Point2 projected = new Utils.Point2(center.x + velocity.x, center.y + velocity.y);
- collisionLine.SetUp(original, projected);
- List<Entity> entitiesInBounds = quadTree.Query(viewBounds);
- bool canMove = true;
- for (int i = 0; i < entitiesInBounds.Count; i++)
- {
- if (entitiesInBounds[i].GetEntityType() == EntityType.WALL)
- {
- Utils.LineSegment compare = ((Wall)entitiesInBounds[i]).GetLineSegment();
- List<Utils.Point2> intersections = collisionLine.Intersects(compare);
- //If there is a collision, don't move.
- if (intersections.Count > 0)
- {
- canMove = false;
- break;
- }
- }
- }
- if (canMove)
- {
- drawingVec.x += velocity.x;
- drawingVec.y += velocity.y;
- }
- for (int i = 0; i < modulationSchemes.Count; i++)
- {
- if (!userControlled)
- modulationSchemes[i](this, entitiesInBounds, modulationSignals[i]);
- else
- {
- for (int j = 0; j < modulationSignals[i].Count; j++)
- ModulationSignal.SetSignal(modulationSignals[i][j], ModulationSignal.NO_MODULATION);
- }
- }
- brain.Train();
- //Update sensors.
- for (int i = 0; i < rangeFinderGroups.Count; i++)
- rangeFinderGroups[i].Update();
- for (int i = 0; i < pieSliceSensorGroups.Count; i++)
- pieSliceSensorGroups[i].Update();
- }
- public override void UpdateEvent(Event e)
- {
- base.UpdateEvent(e);
- }
- public override void Draw()
- {
- base.Draw();
- GL.PushMatrix();
- GL.Translate(center.x, center.y, Utils.DISCARD_Z_POS);
- GL.Rotate(angle, 0.0, 0.0, 1.0);
- GL.Translate(-center.x, -center.y, -Utils.DISCARD_Z_POS);
- GL.Translate(drawingVec.x, drawingVec.y, Utils.DISCARD_Z_POS);
- GL.Scale(width, height, Utils.DISCARD_Z_SCALE);
- GL.DrawElements(mesh.GetRenderMode(), mesh.GetIndexCount(), DrawElementsType.UnsignedShort, IntPtr.Zero);
- GL.PopMatrix();
- for (int i = 0; i < rangeFinderGroups.Count; i++)
- rangeFinderGroups[i].Draw();
- for (int i = 0; i < pieSliceSensorGroups.Count; i++)
- pieSliceSensorGroups[i].Draw();
- }
- public override void DebugDraw()
- {
- base.DebugDraw();
- }
- public override void Clean()
- {
- RangeFinderGroup.Clean();
- PieSliceSensorGroup.Clean();
- }
- public void Reset()
- {
- if (brain != null)
- brain.Reset();
- for (int x = 0; x < modulationSignals.Count; x++)
- {
- for (int y = 0; y < modulationSignals[x].Count; y++)
- ModulationSignal.SetSignal(modulationSignals[x][y], ModulationSignal.NO_MODULATION);
- }
- ModulationScheme.Reset();
- }
- public bool LoadConfig(string sensorFile, string networkFile)
- {
- bool loadSucceeded = true;
- //If a configuration was already loaded delete the
- //VBOs and IBOs used as new ones will be allocated.
- if (configLoaded)
- {
- RangeFinderGroup.Clean();
- PieSliceSensorGroup.Clean();
- configLoaded = false;
- }
- if (!string.IsNullOrEmpty(sensorFile))
- {
- if (!InitSensors(sensorFile))
- loadSucceeded = false;
- }
- if (!string.IsNullOrEmpty(networkFile))
- {
- if (!InitBrain(networkFile))
- loadSucceeded = false;
- }
- configLoaded = true;
- return loadSucceeded;
- }
- public uint GetRangeFinderCount()
- {
- return rangeFinderCount;
- }
- public uint GetPieSliceSensorCount()
- {
- return pieSliceSensorCount;
- }
- public List<List<int>> GetNeuronGroupIds()
- {
- return orderdGroupIds;
- }
- public NeuralNetwork GetBrain()
- {
- return brain;
- }
- private bool InitSensors(string sensorFile)
- {
- if (!File.Exists(sensorFile))
- {
- Console.WriteLine(string.Format(Utils.FILE_NOT_FOUND, sensorFile));
- return false;
- }
- TextReader configReader = new StreamReader(sensorFile);
- SensorConfig sensorConfig = null;
- try
- {
- XmlSerializer deserializer = new XmlSerializer(typeof(SensorConfig));
- sensorConfig = (SensorConfig)deserializer.Deserialize(configReader);
- }
- catch (Exception e)
- {
- Console.WriteLine(Utils.XML_READ_ERROR);
- Console.WriteLine(Utils.SENSOR_LOAD_ERROR);
- Console.WriteLine(e.Message);
- return false;
- }
- finally
- {
- configReader.Close();
- }
- if (sensorConfig.rangeFinderGroups != null)
- {
- for (int i = 0; i < sensorConfig.rangeFinderGroups.Length; i++)
- {
- RangeFinderGroupConfig current = sensorConfig.rangeFinderGroups[i];
- if (current == null)
- continue;
- rangeFinderCount += current.count;
- RangeFinderGroup rfg = new RangeFinderGroup(context, this, quadTree, current.count);
- rfg.Configure(current.length, current.angleOffset, current.angleBetween);
- if (current.entitiesToDetect != null)
- {
- for (int n = 0; n < current.entitiesToDetect.Length; n++)
- {
- Entity.EntityType type = Entity.GetTypeFromString(current.entitiesToDetect[n]);
- if (type != Entity.EntityType.NONE)
- rfg.AddEntityToDetect(type);
- }
- }
- rangeFinderGroups.Add(rfg);
- }
- }
- if (sensorConfig.pieSliceSensorGroups != null)
- {
- for (int i = 0; i < sensorConfig.pieSliceSensorGroups.Length; i++)
- {
- PieSliceSensorGroupConfig current = sensorConfig.pieSliceSensorGroups[i];
- if (current == null)
- continue;
- pieSliceSensorCount += current.count;
- PieSliceSensorGroup pieGroup = new PieSliceSensorGroup(context, this, quadTree);
- pieGroup.AddSensors(current.count);
- pieGroup.ConfigureSensors(current.maxDetection, current.angleOffset, current.angleBetween, current.outerRadius, current.innerRadius);
- if (current.entitiesToDetect != null)
- {
- for (int n = 0; n < current.entitiesToDetect.Length; n++)
- {
- Entity.EntityType type = Entity.GetTypeFromString(current.entitiesToDetect[n]);
- if (type != Entity.EntityType.NONE)
- pieGroup.AddEntityToDetect(type);
- }
- }
- pieSliceSensorGroups.Add(pieGroup);
- }
- }
- return true;
- }
- private bool InitBrain(string networkFile)
- {
- if (!File.Exists(networkFile))
- {
- Console.WriteLine(Utils.FILE_NOT_FOUND, networkFile);
- //brain must be instantiated before leaving.
- brain = new NeuralNetwork();
- return false;
- }
- TextReader configReader = new StreamReader(networkFile);
- NeuralNetworkConfig networkConfig = null;
- try
- {
- XmlSerializer deserializer = new XmlSerializer(typeof(NeuralNetworkConfig));
- networkConfig = (NeuralNetworkConfig)deserializer.Deserialize(configReader);
- }
- catch (Exception e)
- {
- Console.WriteLine(Utils.XML_READ_ERROR);
- Console.WriteLine(Utils.NETWORK_LOAD_ERROR);
- Console.WriteLine(e.Message);
- brain = new NeuralNetwork();
- return false;
- }
- finally
- {
- configReader.Close();
- }
- if (networkConfig.historyBufferSize > 0)
- brain = new NeuralNetwork(networkConfig.historyBufferSize, networkConfig.useNovelty);
- else
- brain = new NeuralNetwork();
- brain.SetOutputNoiseMagnitude(networkConfig.outputNoiseMagnitude);
- brain.SetWeightNoiseMagnitude(networkConfig.weightNoiseMagnitude);
- //No neuron groups, connection groups, or control scheme.
- if (networkConfig.neuronGroups == null)
- {
- Console.WriteLine(Utils.NO_NEURON_GROUPS);
- return false;
- }
- if (networkConfig.connectionGroups == null)
- {
- Console.WriteLine(Utils.NO_CONNECTION_GROUPS);
- return false;
- }
- if (networkConfig.controlScheme == null)
- {
- Console.WriteLine(Utils.NO_CONTROL_SCHEME);
- return false;
- }
- if (networkConfig.weightCap != 0.0)
- brain.SetWeightCap(networkConfig.weightCap);
- ControlScheme.Scheme cDescriptor = ControlScheme.GetSchemeFromString(networkConfig.controlScheme);
- if (cDescriptor == ControlScheme.Scheme.NONE)
- {
- Console.WriteLine(Utils.NO_CONTROL_SCHEME);
- return false;
- }
- controlScheme = ControlScheme.GetSchemeFunction(cDescriptor);
- //Save the modulation descriptions in addition to the functions
- //because the parameter interpreting function takes the descriptions.
- List<ModulationScheme.Scheme> mDescriptions = new List<ModulationScheme.Scheme>();
- List<NeuronGroupConfig> neuronGroups = new List<NeuronGroupConfig>(networkConfig.neuronGroups.Length);
- List<int> neuronGroupIds = new List<int>(networkConfig.neuronGroups.Length);
- for (int i = 0; i < networkConfig.neuronGroups.Length; i++)
- neuronGroups.Add(networkConfig.neuronGroups[i]);
- //Add each neuron group.
- for (int i = 0; i < neuronGroups.Count; i++)
- {
- NeuronGroupConfig nGroupConfig = neuronGroups[(int)i];
- NeuralNetwork.NeuronGroup.Type type = Utils.GetGroupTypeFromString(nGroupConfig.type);
- if (type == NeuralNetwork.NeuronGroup.Type.NONE)
- neuronGroups.RemoveAt(i);
- else
- {
- int index = brain.AddNeuronGroup(nGroupConfig.count, type);
- neuronGroupIds.Add(index);
- orderdGroupIds[(int)type].Add(index);
- }
- }
- //Add each connection group.
- for (int i = 0; i < networkConfig.connectionGroups.Length; i++)
- {
- ConnectionConfig cGroupConfig = networkConfig.connectionGroups[i];
- uint inputGroupIndex = GetIdIndex(cGroupConfig.inputGroupId, neuronGroups);
- uint outputGroupIndex = GetIdIndex(cGroupConfig.outputGroupId, neuronGroups);
- //Make sure the neuron groups to be connected exist.
- if (inputGroupIndex < networkConfig.neuronGroups.Length
- && outputGroupIndex < networkConfig.neuronGroups.Length)
- {
- NeuralNetwork.NeuronGroup.Identifier inputGroup;
- inputGroup.index = neuronGroupIds[(int)inputGroupIndex];
- string inputTypeString = neuronGroups[(int)inputGroupIndex].type;
- inputGroup.type = Utils.GetGroupTypeFromString(inputTypeString);
- NeuralNetwork.NeuronGroup.Identifier outputGroup;
- outputGroup.index = neuronGroupIds[(int)outputGroupIndex];
- string outputTypeString = neuronGroups[(int)outputGroupIndex].type;
- outputGroup.type = Utils.GetGroupTypeFromString(outputTypeString);
- NeuralNetwork.ConnectionGroup.TrainFunctionType trainMethod = Utils.GetMethodFromString(cGroupConfig.trainingMethod);
- //Check if a modulation scheme is specified.
- ModulationScheme.Scheme mDescriptor;
- if (!string.IsNullOrEmpty(cGroupConfig.modulationScheme))
- mDescriptor = ModulationScheme.GetSchemeFromString(cGroupConfig.modulationScheme);
- else
- mDescriptor = ModulationScheme.Scheme.NONE;
- //If a scheme is specified, add a signal for it.
- if (mDescriptor != ModulationScheme.Scheme.NONE)
- {
- uint modSig = ModulationSignal.AddSignal();
- ModulationScheme.SchemeFunction mFunction = ModulationScheme.GetSchemeFunction(mDescriptor);
- //Check if the modulation scheme function has already
- //been added to the list of modulation scheme functions.
- bool hasFunction = false;
- for (int n = 0; n < modulationSchemes.Count; n++)
- {
- if (modulationSchemes[n] == mFunction)
- {
- modulationSignals[n].Add(modSig);
- hasFunction = true;
- break;
- }
- }
- //If the modulation scheme function was not added
- //add it to the list of scheme functions.
- if (!hasFunction)
- {
- mDescriptions.Add(mDescriptor);
- modulationSchemes.Add(mFunction);
- modulationSignals.Add(new List<uint>());
- modulationSignals[modulationSignals.Count - 1].Add(modSig);
- }
- brain.ConnectGroups(inputGroup, outputGroup, trainMethod, (int)modSig,
- cGroupConfig.samplesPerTick, cGroupConfig.learningRate, cGroupConfig.useBias);
- }
- else
- brain.ConnectGroups(inputGroup, outputGroup, trainMethod, ModulationSignal.INVALID_INDEX,
- cGroupConfig.samplesPerTick, cGroupConfig.learningRate, cGroupConfig.useBias);
- }
- }
- //Interpret the parameters of the network file
- //for the control scheme and the modulation schemes.
- if (networkConfig.parameters != null)
- ControlScheme.InterpretParameters(networkConfig.parameters, cDescriptor);
- for (int i = 0; i < mDescriptions.Count; i++)
- ModulationScheme.InterpretParameters(networkConfig.parameters, mDescriptions[i]);
- return true;
- }
- private uint GetIdIndex(uint id, List<NeuronGroupConfig> neuronGroups)
- {
- for (uint i = 0; i < neuronGroups.Count; i++)
- {
- if (id == neuronGroups[(int)i].id)
- return i;
- }
- //Return the first index if the id is not found.
- return 0;
- }
- private Utils.Point2 GetNearestIntersection(List<Utils.Point2> intersections)
- {
- Utils.Point2 nearest = intersections[0];
- for (int x = 1; x < intersections.Count; x++)
- {
- Utils.Point2 currentIntersection = intersections[x];
- Utils.Point2 centerPoint = new Utils.Point2(center.x, center.y);
- if (Utils.GetDist(nearest, centerPoint) > Utils.GetDist(currentIntersection, centerPoint))
- nearest = intersections[x];
- }
- return nearest;
- }
- }
- }
|