public AssociativeTable(Manipulator Agent, int entries)
{
Table = new Dictionary <double, double[]>();
double[] q_init = new double[IKP.ParamNum];
Array.Copy(Agent.q, q_init, IKP.ParamNum);
Random rng = new Random();
IKP Solver = new IKP(0.02, Agent.Links.Length, 10, 0.2, 50);
for (int i = 0; i < 3600; i++)
{
double d = 0.04, a = i; //a = Math.Round(-Math.PI + 2 * Math.PI * rng.NextDouble(), 2);
Vector v = new Vector(d * Math.Cos(a / 10), d * Math.Sin(a / 10));
var res = Solver.Execute(Agent.GripperPos + v);
Array.Copy(q_init, Agent.q, IKP.ParamNum);
while (!res.Item1)
{
res = Solver.Execute(Agent.GripperPos + v);
Array.Copy(q_init, Agent.q, IKP.ParamNum);
}
Table.Add(a, res.Item3);
}
}
public static void RRT(Random rng, ref Tree holder, Point goal, Manipulator manip, Obstacle[] obstacles, int k, double d)
{
Agent = new Manipulator(manip);
Obstacles = obstacles;
holder = new Tree(new Tree.Node(null, Agent.GripperPos, Agent.q));
Attractor[] arr = new Attractor[Manager.Attractors.Count];
Manager.Attractors.CopyTo(arr);
var AttractorsLoc = arr.ToList();
AttractorsLoc.Sort((t, s) => { return(t.Weight <= s.Weight ? (t.Weight < s.Weight ? -1 : 0) : 1); });
for (int i = 0; i < k; i++)
{
/*if ((i + 1) % 5000 == 0)
* {
* tree.RectifyWhole();
* }*/
double radius;
double x;
double y_plus, y_minus;
double y;
double num = Misc.BoxMullerTransform(rng, AttractorsLoc[0].Weight, AttractorsLoc[AttractorsLoc.Count / 2 - 1].Weight / 3);
Attractor attr = AttractorsLoc.Find((t) => { return(t.Weight > num); });
int index = 0;
if (attr == null)
{
index = rng.Next(AttractorsLoc.Count / 2 - 1, AttractorsLoc.Count);
}
else
{
index = AttractorsLoc.IndexOf(attr);
}
//int index = rng.Next(0, Manager.Attractors.Count);
radius = AttractorsLoc[index].Radius;
x = -radius + rng.NextDouble() * 2 * radius;
y_plus = Math.Sqrt(radius * radius - x * x);
y_minus = -y_plus;
y = AttractorsLoc[index].Center.y + (rng.NextDouble() * 2 * (y_plus - y_minus) - (y_plus - y_minus)) / 2;
Point p = new Point(x + AttractorsLoc[index].Center.x, y);
Tree.Node min_node = holder.Min(p);
Vector v = new Vector(min_node.p, p);
Point p_n = min_node.p + v.Normalized * d;
bool collision = false;
foreach (var obst in Obstacles)
{
if (obst.Contains(p_n))
{
collision = true;
break;
}
}
if (!collision)
{
Array.Copy(min_node.q, Algorithm.Agent.q, min_node.q.Length);
var res = Solver.Execute(p_n);
if (res.Item1 && !res.Item4.Contains(true))
{
if (AttractorsLoc[index].InliersCount < 5)
{
Tree.Node node = new Tree.Node(min_node, p_n, Algorithm.Agent.q);
holder.AddNode(node);
if (p_n.DistanceTo(AttractorsLoc[index].Center) < AttractorsLoc[index].Radius)
{
AttractorsLoc[index].InliersCount++;
}
Manager.Buffer.Add(node);
}
else
{
AttractorsLoc.RemoveAt(index);
}
}
}
if (AttractorsLoc[0].InliersCount != 0)
{
break;
}
}
}