public WorldState parse()
{
XmlDocument doc = new XmlDocument();
doc.Load(this.file);
XmlNodeList entities = doc.GetElementsByTagName("SerializedEntities")[0].ChildNodes;
IEnumerator ienum = entities.GetEnumerator();
List<ObjectState> objects = new List<ObjectState>();
int barriers = 0;
while (ienum.MoveNext())
{
XmlNode entity = (XmlNode)ienum.Current;
if (entity.Name == "IRobotCreate")
{
XmlNodeList state = entity.FirstChild.ChildNodes;
XmlNode n = state[0];
string name = n.InnerText;
//Console.WriteLine(name);
string type = "robot";
//Console.WriteLine(type);
float[] position = new float[3];
float[] orientation = new float[3];
float[] velocity = new float[3];
float[] dimension = new float[3];
dimension[0] = 0;
dimension[1] = 0;
dimension[2] = 0;
XmlNodeList pose = state[2].ChildNodes;
XmlNodeList p = pose[0].ChildNodes;
position[0] = float.Parse(p[0].InnerText);
//Console.WriteLine(p[0].InnerText);
position[1] = -float.Parse(p[2].InnerText);
//Console.WriteLine(p[2].InnerText);
position[2] = float.Parse(p[1].InnerText);
//Console.WriteLine(p[1].InnerText);
XmlNodeList o = pose[1].ChildNodes;
orientation[0] = float.Parse(o[0].InnerText);
//Console.WriteLine(o[1].InnerText);
orientation[1] = -float.Parse(o[2].InnerText);
//Console.WriteLine(o[2].InnerText);
orientation[2] = float.Parse(o[1].InnerText);
//Console.WriteLine(o[1].InnerText);
XmlNodeList v = state[3].ChildNodes;
velocity[0] = float.Parse(v[0].InnerText);
//Console.WriteLine(v[0].InnerText);
velocity[1] = -float.Parse(v[2].InnerText);
//Console.WriteLine(v[2].InnerText);
velocity[2] = float.Parse(v[1].InnerText);
//Console.WriteLine(v[1].InnerText);
ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension);
objects.Add(os);
}
//else if (entity.Name == "ENTER ENTITY NAME HERE")
//{
// XmlNodeList state = entity.FirstChild.ChildNodes;
// XmlNode n = state[0];
// string name = n.InnerText;
// //Console.WriteLine(name);
// string type = "robot";
// //Console.WriteLine(type);
// float[] position = new float[3];
// float[] orientation = new float[3];
// float[] velocity = new float[3];
// float[] dimension = new float[3];
// dimension[0] = 0;
// dimension[1] = 0;
// dimension[2] = 0;
// XmlNodeList pose = state[2].ChildNodes;
// XmlNodeList p = pose[0].ChildNodes;
// position[0] = float.Parse(p[0].InnerText);
// //Console.WriteLine(p[0].InnerText);
// position[1] = -float.Parse(p[2].InnerText);
// //Console.WriteLine(p[2].InnerText);
// position[2] = float.Parse(p[1].InnerText);
// //Console.WriteLine(p[1].InnerText);
// XmlNodeList o = pose[1].ChildNodes;
// orientation[0] = float.Parse(o[0].InnerText);
// //Console.WriteLine(o[1].InnerText);
// orientation[1] = -float.Parse(o[2].InnerText);
// //Console.WriteLine(o[2].InnerText);
// orientation[2] = float.Parse(o[1].InnerText);
// //Console.WriteLine(o[1].InnerText);
// XmlNodeList v = state[3].ChildNodes;
// velocity[0] = float.Parse(v[0].InnerText);
// //Console.WriteLine(v[0].InnerText);
// velocity[1] = -float.Parse(v[2].InnerText);
// //Console.WriteLine(v[2].InnerText);
// velocity[2] = float.Parse(v[1].InnerText);
// //Console.WriteLine(v[1].InnerText);
// ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension);
// objects.Add(os);
//}
else if (entity.Name == "SingleShapeEntity")
{
if (barriers == 4)
{
XmlNodeList state = entity.FirstChild.ChildNodes;
XmlNode n = state[0];
string name = n.InnerText;
//Console.WriteLine(name);
string type = "obstacle";
//Console.WriteLine(type);
float[] position = new float[3];
float[] orientation = new float[3];
float[] velocity = new float[3];
float[] dimension = new float[3];
dimension[0] = 0;
dimension[1] = 0;
dimension[2] = 0;
XmlNodeList pose = state[2].ChildNodes;
XmlNodeList p = pose[0].ChildNodes;
position[0] = float.Parse(p[0].InnerText);
//Console.WriteLine(p[0].InnerText);
position[1] = -float.Parse(p[2].InnerText);
//Console.WriteLine(p[2].InnerText);
position[2] = float.Parse(p[1].InnerText);
//Console.WriteLine(p[1].InnerText);
XmlNodeList o = pose[1].ChildNodes;
orientation[0] = float.Parse(o[0].InnerText);
//Console.WriteLine(o[1].InnerText);
orientation[1] = -float.Parse(o[2].InnerText);
//Console.WriteLine(o[2].InnerText);
orientation[2] = float.Parse(o[1].InnerText);
//Console.WriteLine(o[1].InnerText);
XmlNodeList v = state[3].ChildNodes;
velocity[0] = float.Parse(v[0].InnerText);
//Console.WriteLine(v[0].InnerText);
velocity[1] = -float.Parse(v[2].InnerText);
//Console.WriteLine(v[2].InnerText);
velocity[2] = float.Parse(v[1].InnerText);
//Console.WriteLine(v[1].InnerText);
ObjectState os = new ObjectState(name, type, position, orientation, velocity, dimension);
objects.Add(os);
}
else
{
barriers++;
}
}
}
WorldState w = new WorldState(objects);
return w;
}
public WorldPair(WorldDimensions WD, WorldState WS)
{
this.WD = WD;
this.WS = WS;
}
//NEW!!!!! Changed!
public void SetWorldState(WorldState World)
{
this.W = World;
}
//NEW!!!!!! Changed!
public void PopulateWorld(WorldDimensions WD, WorldState World, GenAlgorithm Algo)
{
// Orient sim camera view point
this.SetupCamera(WD);
AddSky();
AddGround(Algo.GridSquareSize);
AddBoundaries(WD.xdim, WD.zdim);
for (int i = 0; i < World.objects.Count; i++)
{
if (World.objects[i].type == "obstacle")
{
AddObstacle(World.objects[i].dimension, World.objects[i].position, World.objects[i].name);
}
else if (World.objects[i].type == "robot")
{
this.RobotList.Add(AddRobot(World.objects[i]));
}
else if (World.objects[i].type == "food")
{
AddFood(World.objects[i].position, World.objects[i].name);
}
}
}
public WorldState GetWorldState()
{
List<ObjectState> objs = new List<ObjectState>();
string name = "";
string type = "obstacle";
float[] position = new float[3];
float[] orientation = new float[3];
float[] velocity = new float[3];
float[] dimension = new float[3];
double newdegreesfromx;
for (int i = 0; i < this.BoundaryList.Count; i++)
{
name = this.BoundaryList[i].State.Name;
if (i < 4)
{
type = "Wall";
}
else
{
type = "obstacle";
}
position[0] = this.BoundaryList[i].State.Pose.Position.X;
position[1] = -this.BoundaryList[i].State.Pose.Position.Z;
position[2] = this.BoundaryList[i].State.Pose.Position.Y;
orientation[0] = this.BoundaryList[i].State.Pose.Orientation.X;
orientation[1] = -this.BoundaryList[i].State.Pose.Orientation.Z;
orientation[2] = this.BoundaryList[i].State.Pose.Orientation.Y;
velocity[0] = this.BoundaryList[i].State.Velocity.X;
velocity[1] = -this.BoundaryList[i].State.Velocity.Z;
velocity[2] = this.BoundaryList[i].State.Velocity.Y;
for (int j = 0; j < this.W.objects.Count; j++)
{
if (name == this.W.objects[j].name)
{
dimension[0] = this.W.objects[j].dimension.X;
dimension[1] = this.W.objects[j].dimension.Y;
dimension[2] = this.W.objects[j].dimension.Z;
}
}
ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension);
objs.Add(o);
}
for (int i = 0; i < this.RobotList.Count; i++)
{
name = this.RobotList[i].State.Name;
type = "robot";
position[0] = this.RobotList[i].State.Pose.Position.X;
position[1] = -this.RobotList[i].State.Pose.Position.Z;
position[2] = this.RobotList[i].State.Pose.Position.Y;
orientation[0] = this.RobotList[i].State.Pose.Orientation.X;
orientation[1] = -this.RobotList[i].State.Pose.Orientation.Z;
orientation[2] = this.RobotList[i].State.Pose.Orientation.Y;
velocity[0] = this.RobotList[i].State.Velocity.X;
velocity[1] = -this.RobotList[i].State.Velocity.Z;
velocity[2] = this.RobotList[i].State.Velocity.Y;
dimension[0] = 1;
dimension[1] = 1;
dimension[2] = 1;
double x = this.RobotList[i].State.Pose.Orientation.X / Math.Sqrt(1 - this.RobotList[i].State.Pose.Orientation.W * this.RobotList[i].State.Pose.Orientation.W);
//Wrong orientation when negative
double y = -(this.RobotList[i].State.Pose.Orientation.Z / Math.Sqrt(1 - this.RobotList[i].State.Pose.Orientation.W * this.RobotList[i].State.Pose.Orientation.W));
//Console.WriteLine(this.RobotList[i].Rotation);
//This is correct!!!
newdegreesfromx = (this.RobotList[i].Rotation.Y + 90);
if (newdegreesfromx > 180) newdegreesfromx = 360 - newdegreesfromx;
ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension);
Console.WriteLine("Orientation is: " + newdegreesfromx);
o.SetNewDegreesFromX(newdegreesfromx);
objs.Add(o);
}
for (int i = 0; i < this.FoodList.Count; i++)
{
name = this.FoodList[i].State.Name;
type = "FoodUnit";
position[0] = this.FoodList[i].State.Pose.Position.X;
position[1] = -this.FoodList[i].State.Pose.Position.Z;
position[2] = this.FoodList[i].State.Pose.Position.Y;
orientation[0] = this.FoodList[i].State.Pose.Orientation.X;
orientation[1] = -this.FoodList[i].State.Pose.Orientation.Z;
orientation[2] = this.FoodList[i].State.Pose.Orientation.Y;
velocity[0] = this.FoodList[i].State.Velocity.X;
velocity[1] = -this.FoodList[i].State.Velocity.Z;
velocity[2] = this.FoodList[i].State.Velocity.Y;
dimension[0] = 0.5f;
dimension[1] = 0.4f;
dimension[2] = 0.5f;
ObjectState o = new ObjectState(name, type, position, orientation, velocity, dimension);
objs.Add(o);
}
WorldState ws = new WorldState(objs);
this.W = ws;
// create an updated world state
// set the simulator field
return W;
}
//NEW!!!!! Changed!
public void ExecuteActions(RobotActions ActionsVector)
{
//Console.WriteLine("SEND RECEIVED ACTION");
this.W = this.RobotsAct(ActionsVector);
}
