fix: search ouf of area

Author: bolom009

demo/recast/main.go

@@ -156,15 +156,15 @@ func main() {
 			}
 		}
 
-		drawTriangles(triangles, rl.LightGray, true)
+		drawTriangles(triangles, rl.LightGray)
 
 		for _, extraObstacle := range extraObstacles {
 			drawArea(extraObstacle.Points(), rl.Gray)
 		}
 
-		//if isDrawGraph {
-		//	drawGraph(pathfinder.GraphWithSearchPath(graphId, start, dest))
-		//}
+		if isDrawGraph {
+			drawGraph(pathfinder.Graph(graphId))
+		}
 		drawPath(start, dest, path, camera.Zoom, true)
 
 		rl.EndMode2D()
@@ -226,19 +226,13 @@ func drawPath(start, dest geom.Vector2, path []geom.Vector2, zoom float32, skipN
 	}
 }
 
-func drawTriangles(triangles []recast.Triangle, color rl.Color, drawEdges ...bool) {
+func drawTriangles(triangles []recast.Triangle, color rl.Color) {
 	for _, pp := range triangles {
 		rl.DrawTriangleFan([]rl.Vector2{
 			rl.NewVector2(pp[0].X, -pp[0].Y),
 			rl.NewVector2(pp[1].X, -pp[1].Y),
 			rl.NewVector2(pp[2].X, -pp[2].Y),
 		}, color)
-
-		if len(drawEdges) > 0 {
-			rl.DrawLineV(rl.NewVector2(pp[0].X, -pp[0].Y), rl.NewVector2(pp[1].X, -pp[1].Y), rl.Gray)
-			rl.DrawLineV(rl.NewVector2(pp[1].X, -pp[1].Y), rl.NewVector2(pp[2].X, -pp[2].Y), rl.Gray)
-			rl.DrawLineV(rl.NewVector2(pp[0].X, -pp[0].Y), rl.NewVector2(pp[2].X, -pp[2].Y), rl.Gray)
-		}
 	}
 }
 
@@ -273,7 +267,7 @@ func drawArea(polygon []geom.Vector2, color rl.Color) {
 func drawGraph(graph map[geom.Vector2][]geom.Vector2) {
 	for p, elems := range graph {
 		for _, elem := range elems {
-			rl.DrawLine(int32(p.X), int32(p.Y), int32(elem.X), int32(elem.Y), rl.NewColor(230, 41, 55, 30))
+			rl.DrawLineV(rl.NewVector2(p.X, -p.Y), rl.NewVector2(elem.X, -elem.Y), rl.DarkGray)
 		}
 	}
 }

graphs/recast/recast.go

@@ -32,6 +32,7 @@ type Recast struct {
 	// extra obstacles
 	obstaclePool                *obstaclePool
 	extraClippedPolygons        []*mesh.Polygon
+	extraEdges                  []*edge
 	extraObstacleId2ClippedPoly map[obstaclePolyPair]goclipper2.PathsD
 }
 
@@ -443,12 +444,25 @@ func (r *Recast) generateGraph() graphs.Graph[geom.Vector2] {
 
 func (r *Recast) prepareEdges(edgeLen int) {
 	r.edges = make([]*edge, 0, edgeLen)
-	for _, polygon := range r.extraClippedPolygons {
+	for _, polygon := range r.polygons {
 		r.edges = append(r.edges, polyToEdges(polygon.Points())...)
 		for _, hole := range polygon.Holes() {
 			r.edges = append(r.edges, polyToEdges(hole.Points())...)
 		}
 	}
+
+	extraObstacles := r.obstaclePool.GetList()
+	for _, extraObstacle := range extraObstacles {
+		r.edges = append(r.edges, polyToEdges(extraObstacle.Points())...)
+	}
+
+	r.extraEdges = make([]*edge, 0, edgeLen)
+	for _, polygon := range r.extraClippedPolygons {
+		r.extraEdges = append(r.extraEdges, polyToEdges(polygon.Points())...)
+		for _, hole := range polygon.Holes() {
+			r.extraEdges = append(r.extraEdges, polyToEdges(hole.Points())...)
+		}
+	}
 }
 
 func polyToEdges(points []geom.Vector2) []*edge {
@@ -588,26 +602,53 @@ func onSegment(p, q, r geom.Vector2) bool {
 	return qX <= math.Max(pX, rX) && qX >= math.Min(pX, rX) && qY <= math.Max(pY, rY) && qY >= math.Min(pY, rY)
 }
 
+// pointOnSegment helper function for check point on segment with epsilon
+func pointOnSegment(p, a, b geom.Vector2, eps float64) bool {
+	var (
+		pX, pY = float64(p.X), float64(p.Y)
+		aX, aY = float64(a.X), float64(a.Y)
+		bX, bY = float64(b.X), float64(b.Y)
+	)
+
+	if pX < math.Min(aX, bX)-eps || pX > math.Max(aX, bX)+eps ||
+		pY < math.Min(aY, bY)-eps || pY > math.Max(aY, bY)+eps {
+		return false
+	}
+
+	cross := (b.X-a.X)*(p.Y-a.Y) - (b.Y-a.Y)*(p.X-a.X)
+	return math.Abs(float64(cross)) <= eps
+}
+
 func (r *Recast) getVisiblePoints(point geom.Vector2) []geom.Vector2 {
-	visiblePoints := make([]geom.Vector2, len(r.vertices))
+	visiblePoints := make([]geom.Vector2, len(r.extraEdges))
 	count := 0
 
-	// TODO better but not so accurate :( ~99853ns
-	for _, v := range r.vertices {
-		hasIntersection := false
-		for _, edge := range r.edges {
-			if lineSegmentIntersection(edge.a, edge.b, v, point) {
-				hasIntersection = true
-				break
-			}
-		}
-
-		if !hasIntersection {
-			visiblePoints[count] = v
+	// TODO very fast check ~11146ns
+	for _, edge := range r.extraEdges {
+		if pointOnSegment(point, edge.a, edge.b, 1e-1) {
+			visiblePoints[count] = edge.a
+			count++
+			visiblePoints[count] = edge.b
 			count++
 		}
 	}
 
+	// TODO better but not so accurate :( ~99853ns
+	//for _, v := range r.vertices {
+	//	hasIntersection := false
+	//	for _, edge := range r.edges {
+	//		if lineSegmentIntersection(edge.a, edge.b, v, point) {
+	//			hasIntersection = true
+	//			break
+	//		}
+	//	}
+	//
+	//	if !hasIntersection {
+	//		visiblePoints[count] = v
+	//		count++
+	//	}
+	//}
+
 	// TODO more accurate but expensive :( ~190585ns
 	//for _, polygon := range r.polygons {
 	//	points := polygon.Points()
@@ -634,7 +675,7 @@ func (r *Recast) closestPointOnPolygon(startPoint geom.Vector2) (geom.Vector2, b
 	exist := false
 
 	// Check exterior ring
-	for _, polygon := range r.clippedPolygons {
+	for _, polygon := range r.extraClippedPolygons {
 		clippedPolygon := polygon.Points()
 		for i := 0; i < len(clippedPolygon); i++ {
 			p1 := clippedPolygon[i]