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)); }