diff --git a/collision.go b/collision.go index 7bd6bb7..1909e04 100644 --- a/collision.go +++ b/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) +} diff --git a/govector_test.go b/govector_test.go index 129f665..3fe930b 100644 --- a/govector_test.go +++ b/govector_test.go @@ -300,3 +300,39 @@ func TestPolyIntersectTwoHits(t *testing.T) { t.Errorf("PolyIntersect Error") } } + +func TestPolyPolyIntersect(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}) + + p2 := Polygon2d{} + p2.Origin = Point2d{5, 5} + p2.Points = append(p2.Points, Vector2d{0, 0}) + p2.Points = append(p2.Points, Vector2d{0, 10}) + p2.Points = append(p2.Points, Vector2d{10, 10}) + p2.Points = append(p2.Points, Vector2d{10, 0}) + + i, m, p := PolyPolyIntersection(p1, Vector2d{0, 0}, p2) + fmt.Printf("%v %v %v\n", i, m, p) +} + +func TestPolyPolyIntersectFail(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}) + + p2 := Polygon2d{} + p2.Origin = Point2d{15, 15} + p2.Points = append(p2.Points, Vector2d{0, 0}) + p2.Points = append(p2.Points, Vector2d{0, 10}) + p2.Points = append(p2.Points, Vector2d{10, 10}) + p2.Points = append(p2.Points, Vector2d{10, 0}) + + i, m, p := PolyPolyIntersection(p1, Vector2d{0, 0}, p2) + fmt.Printf("%v %v %v\n", i, m, p) +} diff --git a/vectorpoint.go b/vectorpoint.go index 6cf566e..2350c0e 100644 --- a/vectorpoint.go +++ b/vectorpoint.go @@ -16,12 +16,16 @@ type Polygon2d struct { } func (p Polygon2d) Edge(i int) (Point2d, Vector2d) { - if i >= len(p.Points)-1 { - return Point2d{}, Vector2d{} - } + p1 := Point2d{} + p2 := Point2d{} - p1 := p.Origin.Add(p.Points[i]) - p2 := p.Origin.Add(p.Points[i+1]) + if i == len(p.Points)-1 { + p1 = p.Origin.Add(p.Points[i]) + p2 = p.Origin.Add(p.Points[0]) + } else { + p1 = p.Origin.Add(p.Points[i]) + p2 = p.Origin.Add(p.Points[i+1]) + } return p1, p2.Sub(p1) } @@ -71,6 +75,10 @@ func (v1 Vector2d) Dot(v2 Vector2d) float32 { return (v1.X * v2.X) + (v1.Y * v2.Y) } +func (p Point2d) ToVector() Vector2d { + return Vector2d{p.X, p.Y} +} + func (v1 Vector2d) Rotate(a float32) Vector2d { x := float64(v1.X)*math.Cos(float64(a)) - float64(v1.Y)*math.Sin(float64(a)) y := float64(v1.X)*math.Sin(float64(a)) + float64(v1.Y)*math.Cos(float64(a))