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