package vector 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) if t > 0 && t < 1 && s > 0 && s < 1 { intersection := p1.Add(v1.Scale(t)) return true, &intersection } return false, nil } func RectRectIntersection(r1 AABB2d, r2 AABB2d) bool { return (r1.A.X < r2.B.X && r1.B.X > r2.A.X && r1.A.Y < r2.B.Y && r1.B.Y > r2.A.Y) } // returns collision, inside, point of collision func RectIntersection(r AABB2d, p Point2d, v Vector2d) (bool, bool, *Point2d) { collision := false inside := false start_inside := PointInRect(p, r) end_inside := PointInRect(p.Add(v), r) if start_inside && end_inside { inside = true return collision, inside, nil } else { wall_left, col1 := Intersection(p, r.A, v, Vector2d{X: 0, Y: r.B.Y - r.A.Y}) if wall_left { return wall_left, inside, col1 } wall_top, col2 := Intersection(p, r.B, v, Vector2d{X: -r.B.X + r.A.X, Y: 0}) if wall_top { return wall_top, inside, col2 } wall_right, col3 := Intersection(p, r.B, v, Vector2d{X: 0, Y: -r.B.Y + r.A.Y}) if wall_right { return wall_right, inside, col3 } wall_bottom, col4 := Intersection(p, r.A, v, Vector2d{X: r.B.X - r.A.X, Y: 0}) if wall_bottom { return wall_bottom, inside, col4 } return false, false, nil } } func ProjectPolygon(axis Vector2d, p Polygon2d) (min float64, max float64) { min = axis.Dot(p.Origin.Add(p.Points[0]).ToVector()) max = min for _, point := range p.Points { d := axis.Dot(p.Origin.Add(point).ToVector()) if d < min { min = d } else { if d > max { max = d } } } return min, max } 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 var iPoint *Point2d distance_sq := math.MaxFloat64 for point, _ := range r.Points { edgePt, edgeVec := r.Edge(point) i, pt := Intersection(p, edgePt, v, edgeVec) if i { intersection = true d_sq := pt.Sub(p).MagSquared() if d_sq < distance_sq { iPoint = pt distance_sq = d_sq } } } return intersection, iPoint } func PointInPolygon(p Point2d, py Polygon2d) bool { // Is p inside py? return false } func IntervalDistance(p1min, p1max, p2min, p2max float64) float64 { if p1min < p2min { return p2min - p1max } else { return p1min - p2max } } // returns intersect, will intersect, translation.Scale(minimum_interval_distance) 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 := math.MaxFloat64 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 := math.Abs(dist) if interval_distance < minimum_interval_distance { minimum_interval_distance = interval_distance translation = axis if p1.Origin.Sub(p2.Origin).Dot(axis) < 0 { translation = translation.Scale(-1) } } } return intersect, will_intersect, translation.Scale(minimum_interval_distance) }