public Tuple <bool, double, double[], bool[]> Execute(Point goal)
{
Goal = goal;
/*Chs = new Chromosome<double>[GenSize];
* Fit = new double[GenSize];
*
* for (int i = 0; i < GenSize; i++)
* {
* Chs[i] = new Chromosome<double>(ParamNum);
* for (int j = 0; j < ParamNum; j++)
* Chs[i].Genes[j] = Agent.StepRanges[j, 0] + (Agent.StepRanges[j, 1] - Agent.StepRanges[j, 0]) * Rng.NextDouble();
* }
*
* Chromosome<double> dominant = null;
* double Min = double.PositiveInfinity;
* bool Converged = false;
* Stopwatch sw = new Stopwatch();
* sw.Start();
* while (Time++ < MaxTime)
* {
* //fitting
* FitnessFunction();
* Array.ConvertAll(Fit, (t) => { return Math.Abs(t); });
*
* //qualification
* Min = Fit.Min();
* if (Min < Precision)
* {
* dominant = Chs[Array.IndexOf(Fit, Min)];
* Converged = true;
* break;
* }
*
* //selection
* RouletteWheelSelection(Chs, Fit);
*
* //mutation
* Mutation(Chs, Pm, t => t + -0.1 + 0.2 * Rng.NextDouble());
*
* //crossover
* Crossover(Chs);
* }
* sw.Stop();
* TotalRealTime += (int)sw.ElapsedTicks / 10;
* Console.WriteLine("IKP Time: {0}; Real time: {1}", Time, sw.ElapsedTicks / 10);
*
* //if dominant chromosome wasn't found, consider last result the best result
* if (dominant == null)
* {
* dominant = Chs[Array.IndexOf(Fit, Min)];
* }
*
* //decoding chromosome
* double[] dq = dominant.GetParamAll(Decode);
*
* //detect all collisions
* Manipulator AgentNext = new Manipulator(Agent)
* {
* q = Agent.q.Zip(dq, (t, s) => { return t + s; }).ToArray()
* };
* bool[] Collisions = DetectCollisions(AgentNext);
*
* Time = 0;
* return new Tuple<bool, double, double[], bool[]>(Converged, Min, dq, Collisions);*/
double range = 0;
for (int i = 0; i < ParamNum; i++)
{
range = -120 * Math.PI / 180 - Agent.q[i];
Agent.StepRanges[i, 0] = range / 5; // <= -1 ? -1 : range;
range = 120 * Math.PI / 180 - Agent.q[i];
Agent.StepRanges[i, 1] = range / 5; // >= 1 ? 1 : range;
}
Manipulator Contestant = new Manipulator(Agent);
double[] q_init = new double[ParamNum];
Array.Copy(Agent.q, q_init, ParamNum);
double dist = Agent.DistanceTo(Goal), init_dist = dist, coeff = 1;
double Min = double.PositiveInfinity;
bool Converged = false;
Stopwatch sw = new Stopwatch();
sw.Start();
while (Time++ < 1000)
{
Chromosome <double> ch = new Chromosome <double>(ParamNum);
for (int i = 0; i < ParamNum; i++)
{
ch.Genes[i] = Agent.StepRanges[i, 0] + (Agent.StepRanges[i, 1] - Agent.StepRanges[i, 0]) * Rng.NextDouble();
ch.Genes[i] *= coeff;
}
double[] dq = ch.GetParamAll(Decode);
Contestant.q = Agent.q.Zip(dq, (t, s) => { return(t + s); }).ToArray();
double dist_new = Contestant.DistanceTo(Goal);
if (dist_new < dist)
{
Array.Copy(Contestant.q, Agent.q, ParamNum);
Min = dist = dist_new;
coeff = dist / init_dist;
}
if (dist < 0.002)
{
Converged = true;
break;
}
}
sw.Stop();
Console.WriteLine("IKP Time: {0}; Real time: {1}", Time, sw.ElapsedTicks / 10);
bool[] Collisions = DetectCollisions(Agent);
Time = 0;
return(new Tuple <bool, double, double[], bool[]>(Converged, Min, Agent.q.Zip(q_init, (t, s) => { return t - s; }).ToArray(), Collisions));
}
