public int Add(Coord val, Coord previous)
{
Coord left = null;
Coord right = null;
Trajet where = null;
float x = 0;
float y = 0;
float z = 0;
if (previous != null)
{
foreach (Trajet t in World.trajets)
{
try
{
Coord current = t[0];
bool debut = true;
while (!(!debut && current == t[0]) && current.next != null)
{
debut = false;
float x1 = current.x;
float y1 = current.y;
float x2 = current.next.x;
float y2 = current.next.y;
float x3 = val.x;
float y3 = val.y;
float x4 = previous.x;
float y4 = previous.y;
if (x1 == x2)
{
current = current.next;
continue;
}
float d1 = ((y1 - y2) * (x3 - x4) - (y3 - y4) * (x1 - x2));
if (d1 == 0)
{
current = current.next;
continue;
}
x = ((x3 * y4 - x4 * y3) *
(x1 - x2) - (x1 * y2 - x2 * y1) * (x3 - x4))
/ d1;
y = x * ((y1 - y2) / (x1 - x2))
+ ((x1 * y2 - x2 * y1) / (x1 - x2));
float d1x = x1 - x2;
float d1y = y1 - y2;
float dist = d1x * d1x + d1y * d1y;
float dx1 = x1 - x;
float dx2 = x2 - x;
float dy1 = y1 - y;
float dy2 = y2 - y;
if ((dx1 == 0 && dy1 == 0) || (dx2 == 0 && dy2 == 0))
{
current = current.next;
continue;
}
dx1 *= dx1;
dy1 *= dy1;
dx2 *= dx2;
dy2 *= dy2;
if (dx1 + dy1 > dist)
{
current = current.next;
continue;
}
if (dx2 + dy2 > dist)
{
current = current.next;
continue;
}
float d2x = x3 - x4;
float d2y = y3 - y4;
dist = d2x * d2x + d2y * d2y;
float dx3 = x3 - x;
float dx4 = x4 - x;
float dy3 = y3 - y;
float dy4 = y4 - y;
if ((dx3 == 0 && dy3 == 0) || (dx4 == 0 && dy4 == 0))
{
current = current.next;
continue;
}
dx3 *= dx3;
dy3 *= dy3;
dx4 *= dx4;
dy4 *= dy4;
if (dx3 + dy3 > dist)
{
current = current.next;
continue;
}
if (dx4 + dy4 > dist)
{
current = current.next;
continue;
}
//calcul du Z
float z1 = current.z;
float z2 = current.next.z;
float z3 = val.z;
float z4 = previous.z;
/* float x = ( ( x3 * z4 - x4 * z3 ) *
* (x1 - x2) - (x1 * z2 - x2 * z1) * ( x3 - x4 ) )
* / ( ( z1 - z2 ) * ( x3 - x4 ) - ( z3 - z4 ) * ( x1 - x2 ) );*/
z = x * ((z1 - z2) / (x1 - x2))
+ ((x1 * z2 - x2 * z1) / (x1 - x2));
// Console.WriteLine("Intersection en {0};{1};{2}", x, y, z );
left = current;
right = current.next;
where = t;
break;
}
}
catch (Exception e)
{
Console.WriteLine("{0}", e.Message);
}
if (where != null)
{
break;
}
}
if (where != null)
{
Intersection inter1 = new Intersection(
left,
right,
previous,
val,
x, y, z);
val.previous = inter1;
previous.next = inter1;
List.Add(inter1);
Intersection inter2 = new Intersection(
previous,
val,
left,
right,
x, y, z);
left.next = inter2;
right.previous = inter2;
where.Add(inter2);
}
}
return(List.Add(val));
}
