public bool CheckMovement(Vector3 from, Vector3 direction, float radius, int layerMask)
{
Vector2 from2 = new Vector2(from.X, from.Z);
Vector2 direction2 = new Vector2(direction.X, direction.Z);
Vector2 to2 = from2 + direction2;
foreach (Obstacle obstacle in obstacles)
{
if (ObstacleLayerMask.CheckMask(layerMask, obstacle.Layer))
{
Vector2 v0 = obstacle.Start;
Vector2 v1 = obstacle.End;
Vector2 normal2 = Vector2.Normalize(new Vector2((v1 - v0).Y, -(v1 - v0).X));
if (Mathg.Cross2D(v1 - v0, from2 - v0) > 0)
{
normal2 *= -1;
}
v0 += normal2 * radius;
v1 += normal2 * radius;
if (Mathg.Cross2D(v1 - v0, from2 - v0) * Mathg.Cross2D(v1 - v0, to2 - v0) < 0 &&
Mathg.Cross2D(to2 - from2, v0 - from2) * Mathg.Cross2D(to2 - from2, v1 - from2) < 0)
{
return(false);
}
}
}
return(true);
}
public bool CheckMovement(Vector3 from, Vector3 direction, float radius, int layerMask, out Vector3 normal, out float relativeLength)
{
normal = Vector3.Zero;
relativeLength = float.MaxValue;
bool ret = true;
Vector2 from2 = new Vector2(from.X, from.Z);
Vector2 direction2 = new Vector2(direction.X, direction.Z);
Vector2 to2 = from2 + direction2;
foreach (Obstacle obstacle in obstacles)
{
if (ObstacleLayerMask.CheckMask(layerMask, obstacle.Layer))
{
Vector2 v0 = obstacle.Start;
Vector2 v1 = obstacle.End;
Vector2 normal2 = Vector2.Normalize(new Vector2((v1 - v0).Y, -(v1 - v0).X));
Vector2 orth = new Vector2(normal2.Y, -normal2.X);
v0 += (normal2 + orth) * radius;
v1 += (normal2 - orth) * radius;
if (Mathg.Cross2D(v1 - v0, from2 - v0) * Mathg.Cross2D(v1 - v0, to2 - v0) < 0 &&
Mathg.Cross2D(to2 - from2, v0 - from2) * Mathg.Cross2D(to2 - from2, v1 - from2) < 0 &&
Vector2.Dot(normal2, Vector2.Normalize(direction2)) < 0.1f)
{
float t = Vector2.Dot(v0 - from2, normal2) / Vector2.Dot(direction2, normal2);
if (t < relativeLength)
{
relativeLength = t;
normal = new Vector3(normal2.X, 0f, normal2.Y);
}
ret = false;
}
}
}
return(ret);
}
