vector/collision.go

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)
}