public Vector3 Intersection(HalfPlane hp)
{
Vector3 tangent = Vector3.Cross(normal, Vector3.up);
Vector3 tangent2 = Vector3.Cross(hp.normal, Vector3.up);
return(Polygon.IntersectionPointOptimized(point, tangent, hp.point, tangent2));
}
public void TestCellRasterization()
{
var v1 = new Vector2(1.2f, 1.5f);
var v2 = new Vector2(0.55f, -1.55f);
var v3 = new Vector2(-2.4f, -1.1f);
var v4 = new Vector2(-0.5f, 2.3f);
var h1 = new HalfPlane(v1, v2, Vector2.Zero);
var h2 = new HalfPlane(v2, v3, Vector2.Zero);
var h3 = new HalfPlane(v3, v4, Vector2.Zero);
var h4 = new HalfPlane(v4, v1, Vector2.Zero);
var isContains = new Predicate <Vector2>(p => HalfPlane.ContainsInConvex(p, new [] { h1, h2, h3, h4 }));
var boundingBox = new Box2(-2.4f, 2.3f, 1.2f, -1.55f);
var result = Rasterization.ConvexToBlocks(isContains, boundingBox);
var correctAnswer = new[] { new Vector2i(0, 1), new Vector2i(0, 0), new Vector2i(0, -1), new Vector2i(0, -2),
new Vector2i(-1, 1), new Vector2i(-1, 0), new Vector2i(-1, -1),
new Vector2i(-2, 0), new Vector2i(-2, -1) };
Assert.That(result, Is.EquivalentTo(correctAnswer));
//Draw polygon
//UnityEngine.Debug.DrawLine(v1, v2, Color.blue, 10);
//UnityEngine.Debug.DrawLine(v2, v3, Color.blue, 10);
//UnityEngine.Debug.DrawLine(v3, v4, Color.blue, 10);
//UnityEngine.Debug.DrawLine(v4, v1, Color.blue, 10);
//Draw bbox
//DrawRectangle.ForDebug();
}
void TraceHalfPlane(NavAgent agent, HalfPlane halfPlane)
{
Vector3 start = new Vector3(agent.transform.position.x, 1.0f, agent.transform.position.z) + new Vector3(halfPlane.p.x, 0.0f, halfPlane.p.y);
Vector2 lineDir = Vector2.Perpendicular(halfPlane.n);
Debug.DrawLine(start, start + 30.0f * new Vector3(lineDir.x, 0, lineDir.y), Color.yellow);
Debug.DrawLine(start, start - 30.0f * new Vector3(lineDir.x, 0, lineDir.y), Color.yellow);
Debug.DrawLine(start, start + 5.0f * new Vector3(halfPlane.n.x, 0, halfPlane.n.y), Color.cyan);
}
public LineSegment Intersect(HalfPlane halfPlane)
{
Vector2 n = Vector2.Perpendicular(Dir);
float D = halfPlane.n.x * n.y - halfPlane.n.y * n.x;
float Dx = Vector2.Dot(halfPlane.n, halfPlane.p) * n.y - halfPlane.n.y * Vector2.Dot(n, this.p1);
float Dy = halfPlane.n.x * Vector2.Dot(n, this.p1) - Vector2.Dot(halfPlane.n, halfPlane.p) * n.x;
Vector2 intersection = new Vector2(Dx / D, Dy / D);
//Check if intersection happens within bounds of line segment. If line segment is unbounded at one end (this.next is null),
//then we simply create a dummy end point that is far down the direction of the line segment.
if (Vector2.Dot(intersection - this.p1, intersection - this.p2) <= 1e-5f)
{
return(new LineSegment(intersection, -Vector2.Perpendicular(halfPlane.n)));
}
return(null);
}
void OnDrawGizmos()
{
if (_h1 && _h2 && _h3 && _h4)
{
var center = (_h1.position + _h2.position + _h3.position + _h4.position) / 4;
var h1 = new HalfPlane((Vector2)_h1.position, (Vector2)_h2.position, (Vector2)center);
var h2 = new HalfPlane((Vector2)_h2.position, (Vector2)_h3.position, (Vector2)center);
var h3 = new HalfPlane((Vector2)_h3.position, (Vector2)_h4.position, (Vector2)center);
var h4 = new HalfPlane((Vector2)_h4.position, (Vector2)_h1.position, (Vector2)center);
var containsCallCounter = 0;
var isContains = new Predicate <Vector2>(delegate(Vector2 p)
{
containsCallCounter++;
return(HalfPlane.ContainsInConvex(p, new [] { h1, h2, h3, h4 }));
});
var bounds = new Box2(
Mathf.Min(_h1.position.x, _h2.position.x, _h3.position.x, _h4.position.x),
Mathf.Max(_h1.position.z, _h2.position.z, _h3.position.z, _h4.position.z),
Mathf.Max(_h1.position.x, _h2.position.x, _h3.position.x, _h4.position.x),
Mathf.Min(_h1.position.z, _h2.position.z, _h3.position.z, _h4.position.z));
//Draw test polygon
Gizmos.color = Color.white;
Gizmos.DrawLine(_h1.position, _h2.position);
Gizmos.DrawLine(_h2.position, _h3.position);
Gizmos.DrawLine(_h3.position, _h4.position);
Gizmos.DrawLine(_h4.position, _h1.position);
//Draw "world" bounds
DrawRectangle.ForGizmo(bounds, Color.gray);
if (Workmode == Mode.Blocks)
{
//Draw block bounds
var blockBounds = (Bounds2i)bounds;
//Draw block centers inside bounds
foreach (var blockBound in blockBounds)
{
var blockCenter = BlockInfo.GetWorldCenter(blockBound);
DebugExtension.DrawPoint((Vector3)blockCenter, Color.white / 2, 0.1f);
}
var blocks = Rasterization.ConvexToBlocks(isContains, bounds);
Assert.IsTrue(blocks.Length == blocks.Distinct().Count());
//Draw rasterized blocks
foreach (var p in blocks)
{
DrawRectangle.ForGizmo(new Bounds2i(p, 1, 1), Color.green);
}
}
else
{
var vertices = Rasterization.ConvexToVertices(isContains, bounds);
Assert.IsTrue(vertices.Length == vertices.Distinct().Count());
//Draw rasterized vertices
foreach (var p in vertices)
{
DebugExtension.DrawPoint(p, Color.green, 0.1f);
}
}
Debug.LogFormat("Contains calls count {0}", containsCallCounter);
}
}
