line poly intersect should gove closest point if intersection hits two edges

collision.go

@@ -1,5 +1,9 @@
 package govector
 
+import (
+	"math"
+)
+
 func Intersection(p1, p2 Point2d, v1, v2 Vector2d) (bool, Point2d) {
 	t := p2.Sub(p1).Cross(v2) / v1.Cross(v2)
 	s := p2.Sub(p1).Cross(v1) / v1.Cross(v2)
@@ -63,19 +67,30 @@ func ProjectPolygon(axis Vector2d, p Polygon2d) (min float32, max float32) {
 
 func PolygonIntersection(r Polygon2d, p Point2d, v Vector2d) (bool, Point2d) {
 
+	// TODO - need to test point in polygon for start and end to detect wholly
+	// inside situations
+	intersection := false
+	i_point := Point2d{}
+	distance_sq := float32(math.MaxFloat32)
+
 	for point, _ := range r.Points {
 		if point == len(r.Points) {
 			break
 		}
 
 		edgePt, edgeVec := r.Edge(point)
-		intersection, pt := Intersection(p, edgePt, v, edgeVec)
+		i, pt := Intersection(p, edgePt, v, edgeVec)
-		if intersection {
+		if i {
-			return intersection, pt
+			intersection = true
+			d_sq := pt.Sub(p).MagSquared()
+			if d_sq < distance_sq {
+				i_point = pt
+				distance_sq = d_sq
+			}
 		}
 	}
 
-	return false, Point2d{}
+	return intersection, i_point
 }
 
 func PolyPolyIntersection(p1 Polygon2d, v1 Vector2d, p2 Polygon2d, v2 Vector2d) bool {

govector_test.go

@@ -286,3 +286,17 @@ func TestPolyIntersectFail(t *testing.T) {
 		t.Errorf("PolyIntersect Error")
 	}
 }
+
+func TestPolyIntersectTwoHits(t *testing.T) {
+	p1 := Polygon2d{}
+	p1.Points = append(p1.Points, Vector2d{0, 0})
+	p1.Points = append(p1.Points, Vector2d{0, 10})
+	p1.Points = append(p1.Points, Vector2d{10, 10})
+	p1.Points = append(p1.Points, Vector2d{10, 0})
+
+	i, p := PolygonIntersection(p1, Point2d{-1, 5}, Vector2d{20, 0})
+	if !i || (p.X != 0 && p.Y != 5) {
+		fmt.Printf("%v", p)
+		t.Errorf("PolyIntersect Error")
+	}
+}

vectorpoint.go

@@ -51,6 +51,10 @@ func (v Vector2d) Mag() float32 {
 	return float32(math.Abs(math.Sqrt(float64(v.X*v.X) + float64(v.Y*v.Y))))
 }
 
+func (v Vector2d) MagSquared() float32 {
+	return float32(math.Abs(float64(v.X*v.X) + float64(v.Y*v.Y)))
+}
+
 func (v Vector2d) PopPop() float32 {
 	return v.Mag()
 }