// ---- Add Objects -----
public void AddObjectToSceneAtPos(PlaceableObject newObj, float x, float y, float phi, Color color)
{
newObj.objectID = totalObjects++;
newObj.transform.position = new Vector3(x / Eyesim.Scale, newObj.defaultVerticalOffset, y / Eyesim.Scale);
newObj.transform.rotation = Quaternion.Euler(new Vector3(0f, Eyesim.UnityToEyeSimAngle(phi), 0f));
newObj.isInit = true;
if (newObj is Robot)
{
SimManager.instance.AddRobotToScene(newObj as Robot);
}
else if (newObj is WorldObject)
{
SimManager.instance.AddWorldObjectToScene(newObj as WorldObject);
}
else if (newObj is Marker)
{
SimManager.instance.AddMarkerToScene(newObj as Marker);
newObj.transform.rotation = Quaternion.Euler(new Vector3(90f, 0f, 0f));
(newObj as Marker).SetColor(color);
}
else
{
Debug.Log("Error adding objects: Unknown type");
Destroy(newObj.gameObject);
}
}
// Save a State
public void SaveState()
{
defaultState = new List <ObjectState>();
worldChanged = false;
stateID = 0;
foreach (Robot robot in allRobots)
{
ObjectState state = new ObjectState();
state.id = robot.objectID;
state.type = robot.type;
state.pos = Eyesim.GeneratePositionStringFromTransform(robot.transform);
stateID = robot.objectID > stateID ? robot.objectID : stateID;
defaultState.Add(state);
}
foreach (WorldObject wObj in allWorldObjects)
{
ObjectState state = new ObjectState();
state.id = wObj.objectID;
state.type = wObj.type;
state.pos = Eyesim.GeneratePositionStringFromTransform(wObj.transform);
stateID = wObj.objectID > stateID ? wObj.objectID : stateID;
defaultState.Add(state);
}
foreach (Marker mark in allMarkers)
{
ObjectState state = new ObjectState();
state.type = "marker";
state.pos = Eyesim.GeneratePositionString(mark.transform.position.x, mark.transform.position.z, mark.mColor.r) +
":" + mark.mColor.g +
":" + mark.mColor.b +
":" + mark.mColor.a;
state.color = mark.mColor;
defaultState.Add(state);
}
}
// Set the speed of a single motor
public void SetMotorSpeed(int motor, int speed)
{
int factor = Eyesim.ClampInt(speed, -100, 100);
float vSpeed = factor / 100f * maxMotorTorque;
if (motor > driveWheels.Count || motor < 0)
{
Debug.Log("SetMotorSpeed: Bad motor input");
return;
}
driveWheels[motor].SetSpeed(vSpeed);
}
// Update is called once per frame
void Update()
{
if (robot is IPSDSensors)
{
psdLeftValue.text = psdController.GetPSDValue(0).ToString();
psdFrontValue.text = psdController.GetPSDValue(1).ToString();
psdRightValue.text = psdController.GetPSDValue(2).ToString();
}
if (!SimManager.instance.isPaused)
{
robotXValue.text = (Eyesim.Scale * robot.transform.position.x).ToString("N2");
robotZValue.text = (Eyesim.Scale * robot.transform.position.z).ToString("N2");
robotPhiValue.text = Eyesim.UnityToEyeSimAngle(robot.transform.rotation.eulerAngles.y).ToString("N2");
}
if (robot is IVWDrive)
{
Int16[] pos = (robot as IVWDrive).GetPose();
vwXtext.text = pos[0].ToString("N2");
vwYtext.text = pos[1].ToString("N2");
vwPhiText.text = pos[2].ToString("N2");
}
}
// Write scene to a Sim file
public void SaveSim(string filepath)
{
PauseSimulation();
string worldPath;
if (worldFilepath == null)
{
worldPath = Path.Combine(Path.GetDirectoryName(filepath), Path.GetFileNameWithoutExtension(filepath) + "_world.wld");
SaveWorld(worldPath);
}
else
{
worldPath = worldFilepath;
}
FileStream fs = File.Open(filepath, FileMode.Create);
using (StreamWriter writer = new StreamWriter(fs, System.Text.Encoding.ASCII))
{
// Save world, and link to default save location
writer.WriteLine("# Sim file created " + DateTime.Now.ToString(@"MM\/dd\/yyyy h\:mm tt") + Environment.NewLine);
writer.WriteLine("# World File " + Environment.NewLine + "world " + "\"" + worldPath + "\"");
writer.Write(Environment.NewLine + Environment.NewLine);
// Save robot locations
writer.WriteLine("# Robots");
foreach (Robot rob in allRobots)
{
writer.WriteLine(rob.type + " " + (int)Math.Floor(rob.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(rob.transform.position.z * Eyesim.Scale) + " " + (int)Math.Floor(Eyesim.UnityToEyeSimAngle(rob.transform.eulerAngles.y)));
}
writer.WriteLine();
// Save object locations
writer.WriteLine("# Objects");
foreach (WorldObject wObj in allWorldObjects)
{
writer.WriteLine(wObj.type + " " + (int)Math.Floor(wObj.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(wObj.transform.position.z * Eyesim.Scale) + " " + (int)Math.Floor(Eyesim.UnityToEyeSimAngle(wObj.transform.eulerAngles.y)));
}
// Save markers
writer.WriteLine("# Markers");
foreach (Marker mark in allMarkers)
{
writer.WriteLine("marker " + (int)Math.Floor(mark.transform.position.x * Eyesim.Scale) + " " + (int)Math.Floor(mark.transform.position.z * Eyesim.Scale) + " " +
(int)Math.Round(mark.mColor.r * 255) + " " +
(int)Math.Round(mark.mColor.g * 255) + " " +
(int)Math.Round(mark.mColor.b * 255) + " " +
(int)Math.Round(mark.mColor.a * 255));
}
ResumeSimulation();
}
EyesimLogger.instance.Log("Saved sim to file " + filepath);
}
