public void InsertObstacle(Entity key, float4x4 ltw, float2 *vertices, int amount)
{
Assert.IsTrue(amount > 1);
Assert.IsTrue(key != Entity.Null);
Assert.IsTrue(!Map.ContainsKey(key));
Assert.IsTrue(amount > 2 && math.all(vertices[0] == vertices[amount - 1]), "Obstacle needs to be counter cockwise wound and closed");
Obstacle *first = default;
Obstacle *previous = default;
var prevPos = Math.Mul2D(ltw, vertices[0]);
var prevPos2 = Math.Mul2D(ltw, vertices[amount - 2]);
for (var i = 0; i < amount - 1; ++i)
{
var obstacle = ObstaclePool.GetElementPointer();
obstacle->Point = prevPos;
var pos = Math.Mul2D(ltw, vertices[i + 1]);
var aabb = AABB.FromOpposingCorners(prevPos, pos);
obstacle->Id = Tree.Insert(aabb, (IntPtr)obstacle);
obstacle->Direction = math.normalize(pos - prevPos);
if (amount == 2)
{
obstacle->Convex = true;
}
else
{
obstacle->Convex = LeftOf(prevPos2, prevPos, pos) >= 0;
}
prevPos2 = prevPos;
prevPos = pos;
if (i == 0)
{
first = obstacle;
previous = obstacle;
}
else if (i < amount - 2)
{
obstacle->Previous = previous;
previous->Next = obstacle;
previous = obstacle;
}
else
{
obstacle->Previous = previous;
previous->Next = obstacle;
first->Previous = obstacle;
obstacle->Next = first;
}
}
Map.Add(key, (IntPtr)first);
public ObstacleDistance(float dist, Obstacle *obstacle)
{
Dist = dist;
Obstacle = obstacle;
}