public static List <Trajectory> Truncate(this Trajectory trj, Envelope box)
{
List <Trajectory> trjs = new List <Trajectory>();
Trajectory subTrj = new Trajectory();
int minCount = 128;
for (int i = 0; i < trj.Count; ++i)
{
if (box.Contains(trj[i].point.ToCoordinate()))
{
subTrj.Add(trj[i]);
}
else
{
if (subTrj.Count > minCount)
{
trjs.Add(subTrj);
}
subTrj = new Trajectory();
}
}
if (subTrj.Count > minCount)
{
trjs.Add(subTrj);
}
return(trjs);
}
private void CalculateTrajectory()
{
Trajectory.Clear();
Ray ray = new Ray(transform.position, ballRb.velocity.normalized);
RaycastHit hit;
bool endOfCourt = false;
int sentinel = 1000;
while (!endOfCourt && sentinel > 0)
{
if (Physics.Raycast(ray, out hit))
{
if (hit.collider.gameObject.tag == "Wall")
{
Trajectory.Add(hit.point);
var nextDir = Vector3.Reflect(ray.direction, hit.normal);
ray = new Ray(hit.point, nextDir);
}
else if (hit.collider.gameObject.tag == "Goal")
{
Trajectory.Add(hit.point);
endOfCourt = true;
}
else
{
Trajectory.Add(hit.point); // only other object is paddle, so we count as end
endOfCourt = true;
}
}
sentinel--;
}
}
private Trajectory SimulateTrajectory(Vector3 force, Vector3 torque, SimulationBaseData simData, float targetHeight)
{
var simObject = simData.SimObject;
var rigidBody = simData.SimRigidBody;
var trajectory = new Trajectory();
rigidBody.velocity = simData.OriginalRigidBody.velocity;
rigidBody.angularVelocity = simData.OriginalRigidBody.angularVelocity;
rigidBody.useGravity = true;
rigidBody.AddForce(force);
rigidBody.AddTorque(torque);
for (int i = 0; i <= DEFAULT_MAX_ITERATIONS; i++)
{
_simulationPhysicsScene.Simulate(Time.fixedDeltaTime);
var position = simObject.transform.position;
var goingDownwards = rigidBody.velocity.y < 0;
trajectory.Add(position);
if (goingDownwards && position.y <= targetHeight)
{
return(trajectory);
}
}
return(trajectory);
}
public void AddTrajectory(List <Vector> trajectory)
{
for (int i = 0; i < trajectory.Count; i++)
{
Trajectory.Add(trajectory[i].Clone() as Vector);
}
}
public Trajectory Compress()
{
Trajectory refinedTrj = new Trajectory();
int trjSize = trj.Count;
int i = 0;
while (i < trjSize - 1)
{
refinedTrj.Add(trj[i]);
int j = i + 1;
for (; j < trjSize; ++j)
{
if (!canApproximate(i, j))
{
break;
}
}
i = j - 1;
}
refinedTrj.Add(trj[trjSize - 1]);
return(refinedTrj);
}
// Update is called once per frame
void Update()
{
var diceRoll = Random.Range((float)0.0, (float)1.0);
foreach (var vehicle in GameObject.FindGameObjectsWithTag("vehicle"))
{
var vehiclePosition = vehicle.transform.position;
var totalDistance = Vector3.Distance(vehiclePosition, transform.position);
if (totalDistance < MinimumSpawnDistance) //If there's a vehicle within 5 units of this spawn point, don't spawn
{
return;
}
}
if (diceRoll < Probability)
{
var newcar = Instantiate(GameObject.Find("CarPrototype"), transform.position, transform.rotation);
newcar.name = "car";
newcar.tag = "vehicle";
var navigation = newcar.GetComponentInChildren <TargetNavigation>();
for (var i = 0; i < this.transform.root.childCount; i++) //Add exit trajectories under this approach
{
if (this.transform.root.GetChild(i).CompareTag("trajectory"))
{
var t = new Trajectory();
var ti = this.transform.root.GetChild(i).GetComponent <TrajectoryInfo>();
var child = this.transform.root.GetChild(i);
for (var j = 0; j < child.transform.childCount; j++)
{
var tf = child.GetChild(j);
t.Add(tf);
}
t.Road = ti.Road;
t.Turn = ti.Type;
navigation.Exits.Add(t);
}
}
if (navigation.Exits.Count == 0)
{
Console.WriteLine("This should not happen.");
}
navigation.RandomizeTarget();
newcar.GetComponentInChildren <TargetNavigation>().enabled = true;
newcar.GetComponentInChildren <CarMovement>().enabled = true;
}
}
public void Calculate(AlgorithmType t)
{
InitializeProcessToRun(t);
Debug.Assert(processToRun != null);
AbstractNetworkContainer containerToChange = (AbstractNetworkContainer)container.Clone();
Double timeStep = 0;
Double currentTracingStep = tracingStepIncrement, currentActiveNodesCount = containerToChange.GetActiveNodesCount();
Trajectory.Add(timeStep, currentActiveNodesCount / containerToChange.Size);
if (visualMode)
{
Byte[] bits = new Byte[containerToChange.GetActiveStatuses().Length + 1];
containerToChange.GetActiveStatuses().CopyTo(bits, 0);
BitArray arr = new BitArray(bits);
ActivesInformation.Add(arr);
}
while (timeStep <= stepCount && currentActiveNodesCount != 0)
{
processToRun(containerToChange);
currentActiveNodesCount = containerToChange.GetActiveNodesCount();
Trajectory.Add(++timeStep, currentActiveNodesCount / containerToChange.Size);
if (visualMode)
{
Byte[] bits = new Byte[containerToChange.GetActiveStatuses().Length + 1];
containerToChange.GetActiveStatuses().CopyTo(bits, 0);
BitArray arr = new BitArray(bits);
ActivesInformation.Add(arr);
}
network.UpdateStatus(NetworkStatus.StepCompleted);
// TODO closed Trace
/*if (TraceCurrentStep(timeStep, currentTracingStep, "ActivationAlgorithm_1"))
* {
* currentTracingStep += TracingStepIncrement;
* network.UpdateStatus(NetworkStatus.StepCompleted);
* }*/
}
Debug.Assert(timeStep > stepCount || !containerToChange.DoesActiveNodeExist());
}
public Trajectory Decompress(VCompressedTrj ctrj)
{
int size = ctrj.Items.Count;
if (size <= 0)
{
return(null);
}
Trajectory trj = new Trajectory();
// Add the first reference point
for (int i = 0; i < size; ++i)
{
var item = ctrj.Items[i];
double dist = item.RefPoint.distance; // distance from the start point the edge to the current point
long t = item.RefPoint.t;
// add the reference point first
trj.Add(new MotionVector(item.RefPoint.Point, item.RefPoint.t));
Edge lastEdge = item.RefPoint.e;
// Calculate the position of each point
for (int j = 0; j < item.Points.Count; ++j)
{
var cMV = item.Points[j];
double totalDistance = cMV.si * cMV.v * _binSize; // the distance covered during the interval
GeoPoint p = GeoPoint.INVALID;
t += cMV.si;
if (cMV.rid == lastEdge.ID)
{
dist += totalDistance;
p = lastEdge.Predict(lastEdge.Start.Point, dist);
}
else
{
Edge curEdge = _g.Edges[cMV.rid];
dist = totalDistance - (lastEdge.Length - dist); // minus the distance left in the previous road
if (lastEdge.End == curEdge.Start)
{
p = curEdge.Predict(curEdge.Start.Point, dist);
}
else
{
var path = _g.FindPath(lastEdge.End, curEdge.Start);
if (path != null)
{
foreach (var e in path)
{
dist -= e.Length;
}
p = curEdge.Predict(curEdge.Start.Point, dist);
}
else
{
Debug.Assert(false);
}
}
lastEdge = curEdge;
}
trj.Add(new MotionVector(p, t));
}
}
return(trj);
}
public Trajectory GradientDescent()
{
var parameters = this.Parameters;
if (function == null || derivation == null || parameters == null)
{
throw new OperationCanceledException("Неможливо розпочати роботу. Не всі параметри вказано");
}
var curPoint = parameters.StartPoint;
var curBeta = parameters.Beta;
var staticBeta = curBeta;
double curAlpha = 0;
bool pointChanged = false;
Arguments antiGrad = derivation.GetAntiGradient(curPoint);
double functionInCurPoint = function.Calculate(curPoint);
double antiGradModule = antiGrad.Pow(2).GetSumm();
var allPoints = new Trajectory();
allPoints.Add(curPoint);
for (; ;)
{
if (pointChanged)
{
antiGrad = derivation.GetAntiGradient(curPoint);
functionInCurPoint = function.Calculate(curPoint);
antiGradModule = antiGrad.Pow(2).GetSumm();
}
Arguments nextPoint = curPoint + (antiGrad * curBeta);
double functionInNextPoint = function.Calculate(nextPoint);
if (!IsValidValue(antiGradModule))
{
return(allPoints);
}
//F(Xk)-F(xk=B*antigrad(xk))>=epsilon|antigrad(xk)|^2
double toTest = antiGradModule * (curBeta * parameters.Epsilon);
double toTestLeft = functionInCurPoint - functionInNextPoint;
if (toTestLeft >= toTest)
{
//add point it is normal
allPoints.Add(nextPoint);
double exitValue = (nextPoint - curPoint).GetModule();
if (exitValue < parameters.AccuracyEpsilon)
{
return(allPoints); //exit
}
else
{
curPoint = nextPoint;
// Increase step
Arguments antiGradNew = derivation.GetAntiGradient(curPoint);
antiGrad = antiGradNew;
bool calculated = false;
Arguments newValue = null;
double funcResultNew = 0;
Arguments oldValue = null;
double funcResultOld = 0;
for (; ;)
{
if (calculated)
{
oldValue = newValue;
funcResultOld = funcResultNew;
}
else
{
oldValue = curPoint + (antiGradNew * curBeta);
funcResultOld = function.Calculate(oldValue);
}
var nextInfinity = Double.IsInfinity(curBeta * parameters.M);
if (!nextInfinity)
{
curBeta = curBeta * parameters.M;
// Step is too big.
newValue = curPoint + (antiGradNew * curBeta);
funcResultNew = function.Calculate(newValue);
}
calculated = true;
if (nextInfinity || funcResultOld <= funcResultNew)
{
curBeta = curBeta / parameters.M;
staticBeta = curBeta;
pointChanged = true;
curPoint = curPoint + (antiGrad * curBeta);
curAlpha = 0;
break;
}
}
}
}
else
{
pointChanged = false;
if (curAlpha == 0)
{
curAlpha = parameters.Alpha;
}
else
{
curAlpha = curAlpha * curAlpha;
}
curBeta = staticBeta * curAlpha;
}
}
}
