poly ploy intersect seems to be working

collision.go

@@ -1,6 +1,7 @@
 package govector
 
 import (
+	"fmt"
 	"math"
 )
 
@@ -50,10 +51,10 @@ func RectIntersection(r AABB2d, p Point2d, v Vector2d) (bool, bool, Point2d) {
 }
 
 func ProjectPolygon(axis Vector2d, p Polygon2d) (min float32, max float32) {
-	min = axis.Dot(p.Points[0])
+	min = axis.Dot(p.Origin.Add(p.Points[0]).ToVector())
 	max = min
 	for _, point := range p.Points {
-		d := axis.Dot(point)
+		d := axis.Dot(p.Origin.Add(point).ToVector())
 		if d < min {
 			min = d
 		} else {
@@ -93,7 +94,77 @@ func PolygonIntersection(r Polygon2d, p Point2d, v Vector2d) (bool, Point2d) {
 	return intersection, i_point
 }
 
-func PolyPolyIntersection(p1 Polygon2d, v1 Vector2d, p2 Polygon2d, v2 Vector2d) bool {
-
+func PointInPolygon(p Point2d, py Polygon2d) bool {
+	// Is p inside py?
 	return false
 }
+
+func IntervalDistance(p1min, p1max, p2min, p2max float32) float32 {
+	if p1min < p2min {
+		return p2min - p1max
+	} else {
+		return p1min - p2max
+	}
+}
+
+func PolyPolyIntersection(p1 Polygon2d, v1 Vector2d, p2 Polygon2d) (bool, bool, Vector2d) {
+	// p1 is moving v1, p2 is stationary
+	intersect := true
+	will_intersect := true
+	translation := Vector2d{}
+	edges := []Vector2d{}
+	minimum_interval_distance := float32(math.MaxFloat32)
+
+	for point, _ := range p1.Points {
+		_, edgeVec := p1.Edge(point)
+		edges = append(edges, edgeVec)
+	}
+
+	for point, _ := range p2.Points {
+		_, edgeVec := p2.Edge(point)
+		edges = append(edges, edgeVec)
+	}
+
+	for _, edgeVec := range edges {
+		axis := Vector2d{-edgeVec.Y, edgeVec.X}.Normalize()
+		p1min, p1max := ProjectPolygon(axis, p1)
+		p2min, p2max := ProjectPolygon(axis, p2)
+		dist := IntervalDistance(p1min, p1max, p2min, p2max)
+
+		if dist > 0 {
+			intersect = false
+		}
+
+		// Now check where it WILL be
+		v_projection := axis.Dot(v1)
+		if v_projection > 0 {
+			p1max += v_projection
+		} else {
+			p1min += v_projection
+		}
+
+		// Look again
+		dist = IntervalDistance(p1min, p1max, p2min, p2max)
+
+		if dist > 0 {
+			will_intersect = false
+		}
+
+		if !intersect && !will_intersect {
+			break
+		}
+
+		interval_distance := float32(math.Abs(float64(dist)))
+		if interval_distance < minimum_interval_distance {
+			minimum_interval_distance = interval_distance
+			translation = axis
+			fmt.Printf(" AXIS: %v %v\n", axis, interval_distance)
+			if p1.Origin.Sub(p2.Origin).Dot(axis) < 0 {
+				translation = translation.Scale(-1)
+			}
+
+		}
+	}
+
+	return intersect, will_intersect, translation.Scale(minimum_interval_distance)
+}