public override void CollectObservations()
{
Spline.Point sp = GetSplinePointAtCar(crntSPIndex);
Vector3 forwardXZ = Vector3.ProjectOnPlane(carCtrl.transform.forward, Vector3.up);
float orientation = Vector3.SignedAngle(forwardXZ, sp.tangentXZ, Vector3.up);
AddVectorObs(orientation / 180f); // 1
float offset = (sp.localPos + sp.normal * road.Width - localCarPos).magnitude;
AddVectorObs((offset / road.Width) * 2f - 1f); // 1
AddVectorObs(Sigmoid(carCtrl.LocalVelocity)); // 3
AddVectorObs(Sigmoid(carCtrl.LocalAngularVelocity)); // 3
AddVectorObs(carCtrl.Inclination); // 3
AddVectorObs(carCtrl.NormalizedSteerAngle); // 1
if (!useCam)
{
AddVectorObs(roadObserver.GetNormalizedObs(sp, carCtrl.transform)); // 50
}
float forwardSpeed = Vector3.Dot(sp.tangent, carCtrl.Velocity);
AddReward(forwardSpeed * speedRewardMultiplier);
}
public override void Deserialize(BinaryReader stream)
{
MeshType type = (MeshType)stream.ReadInt32();
if (type == MeshType.Cube)
{
Value = new Cube(stream.ReadSingle(), stream.ReadInt32());
}
else if (type == MeshType.Sphere)
{
Value = new Sphere(stream.ReadSingle());
}
else if (type == MeshType.Square)
{
Value = new Square(stream.ReadSingle());
}
else if (type == MeshType.Spline)
{
int ptCount = stream.ReadInt32();
Spline.Point[] pts = new Spline.Point[ptCount];
for (int i = 0; i < ptCount; i++)
{
pts[i] = new Spline.Point(new Vector3(stream.ReadSingle(), stream.ReadSingle(), stream.ReadSingle()), new Vector3(stream.ReadSingle(), stream.ReadSingle(), stream.ReadSingle()), new Vector3(stream.ReadSingle(), stream.ReadSingle(), stream.ReadSingle()));
}
Value = new Spline(pts);
}
else if (type == MeshType.HexGrid)
{
Value = new HexGrid(stream.ReadInt32(), stream.ReadInt32(), stream.ReadInt32());
}
else
{
throw new NotImplementedException("Mesh type " + type + " doesn't have a deserializer defined.");
}
}
public void ChangeSpline(Spline.Point[] points)
{
if (null == points)
{
throw new Exception(Exception.Reason.FloatValuesNull, algorithm, null, intValue);
}
if (2 > points.Length)
{
throw new Exception(Exception.Reason.FloatValuesWrongSize, algorithm, new float[0], intValue);
}
var first = points[0];
var last = points[points.Length - 1];
points[0] = new Spline.Point {
key = 0, value = first.value,
};
points[points.Length - 1] = new Spline.Point {
key = 1, value = last.value,
};
var floatValues = new float[points.Length * 2];
for (int n = 0, cnt = points.Length; n < cnt; ++n)
{
floatValues[(n * 2)] = points[n].key;
floatValues[(n * 2) + 1] = points[n].value;
}
this.Change(AlgorithmType.Spline, floatValues, 0);
}
public List <float> GetNormalizedObs(Spline.Point carSP, Transform car)
{
CalcWaypoints(carSP);
normalizedObs.Clear();
for (int i = 0, n = waypoints.Length - 1; i < n; i++)
{
Spline.Point w0 = waypoints[i];
Spline.Point w1 = waypoints[i + 1];
Vector3 delta = w1.localPos - w0.localPos;
float length = delta.magnitude;
// The normalized length of this road segment relative to max segment length.
normalizedObs.Add((length / maxSegmentLength) * 2f - 1f);
// The direction of this road segment in car's local space.
Vector3 direction = car.InverseTransformVector(delta.normalized);
normalizedObs.Add(direction.x);
normalizedObs.Add(direction.y);
normalizedObs.Add(direction.z);
for (int j = 1; j <= numRays; j++)
{
Vector3 p0 = w0.globalPos + w0.normal * road.Width - w0.normal * (j * rayGap) + rayOffset;
Vector3 p1 = w1.globalPos + w1.normal * road.Width - w1.normal * (j * rayGap) + rayOffset;
if (Physics.SphereCast(p0, rayRadius, p1 - p0, out RaycastHit hit, length, lmObstacle))
{
Debug.DrawLine(p0, hit.point, Color.red);
// The normalized obstacle distance relative to current segment length.
normalizedObs.Add((hit.distance / length) * 2f - 1f);
}
public override string Export()
{
if (Value == null)
{
return(MeshType.None.ToString());
}
string output = "";
if (Value is Cube)
{
Cube cube = Value as Cube;
output += MeshType.Cube + "(" + cube.Scale + ", " + cube.Res + ")";
}
else if (Value is Sphere)
{
Sphere sphere = Value as Sphere;
output += MeshType.Sphere + "(" + sphere.Radius + ")";
}
else if (Value is Square)
{
Square square = Value as Square;
output += MeshType.Square + "(" + square.Scale + ")";
}
else if (Value is Spline)
{
Spline spline = Value as Spline;
output += MeshType.Spline + "([";
for (int i = 0; i < spline.Points.Count; i++)
{
Spline.Point pt = spline.Points[i];
output += "(" + pt.Position.X + ", " + pt.Position.Y + ", " + pt.Position.Z + "); ";
output += "(" + pt.LeftControl.X + ", " + pt.LeftControl.Y + ", " + pt.LeftControl.Z + "); ";
output += "(" + pt.RightControl.X + ", " + pt.RightControl.Y + ", " + pt.RightControl.Z + ")";
if (i < spline.Points.Count - 1)
{
output += "; ";
}
}
output += "])";
}
else if (Value is HexGrid)
{
HexGrid hexGrid = Value as HexGrid;
output += MeshType.HexGrid + "(" + hexGrid.Size + ", " + hexGrid.OffX + ", " + hexGrid.OffY + ")";
}
else
{
throw new NotImplementedException("Mesh type " + Value.GetType().Name + " doesn't have an exporter defined.");
}
return(output);
}
public override void InitializeAgent()
{
environment = transform.parent;
spline = road.GetSpline();
crntSPIndex = startSPIndex;
Spline.Point sp = spline.GetPoint(crntSPIndex);
Vector3 pos = sp.globalPos;
pos.y += 0.5f;
carCtrl.transform.position = pos;
carCtrl.transform.rotation = Quaternion.LookRotation(sp.tangent);
resetter = new Resetter(environment);
agentCam.gameObject.SetActive(useCam);
roadObserver.gameObject.SetActive(!useCam);
if (!useCam)
{
roadObserver.Initialize();
}
collisionDetection.CollisionCallback = OnObstacleCollision;
}
private void UpdateMesh()
{
CreateSpline();
mesh = GetComponent <MeshFilter>().sharedMesh;
if (mesh == null)
{
mesh = new Mesh();
mesh.MarkDynamic();
}
else if (cloneMesh)
{
mesh = Instantiate(GetComponent <MeshFilter>().sharedMesh);
}
int n = spline.Count;
Vector3[] vertices = new Vector3[n * 2];
int[] triangles = new int[n * 6];
for (int i = 0, v = 0, t = 0; i < n; i++)
{
bool last = i == n - 1;
triangles[t++] = v;
triangles[t++] = last ? 0 : v + 2;
triangles[t++] = last ? 1 : v + 3;
triangles[t++] = v;
triangles[t++] = last ? 1 : v + 3;
triangles[t++] = v + 1;
Spline.Point sp = spline.GetPoint(i);
vertices[v++] = sp.localPos + sp.normal * Width;
vertices[v++] = sp.localPos + sp.normal * -Width;
}
mesh.Clear();
mesh.vertices = vertices;
mesh.normals = Enumerable.Repeat(Vector3.up, n * 2).ToArray();
mesh.triangles = triangles;
GetComponent <MeshFilter>().sharedMesh = mesh;
GetComponent <MeshCollider>().sharedMesh = mesh;
}
private void UpdateObstacles()
{
if (amount > 0)
{
Spline spline = road.GetSpline();
int n = spline.Count;
int step = n / amount;
float offW = road.Width * 0.5f;
Vector3 scale = Vector3.one * road.Width * size * 0.5f;
Vector3 offH = Vector3.up * scale.y * 0.5f;
for (int i = 0; i < amount; i++)
{
Spline.Point sp = spline.GetPoint(i * step);
Vector3 v = sp.localPos + sp.normal * offW * (Random.value > 0.5f ? 1f : -1f);
Transform cube = GetCube(i);
cube.localScale = scale;
cube.localPosition = v + offH;
cube.rotation = Quaternion.LookRotation(sp.tangent);
cube.gameObject.GetComponent <Rigidbody>().mass = Mathf.Pow(scale.x, 3) * massMultiplier;
}
}
DeactiveUnused(amount);
}
