vector/collision.go

170 lines
3.9 KiB
Go

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)
if t > 0 && t < 1 && s > 0 && s < 1 {
return true, p1.Add(v1.Scale(t))
}
return false, Point2d{0, 0}
}
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)
}
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, p
} 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.A, 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.B, v, Vector2d{X: -r.B.X + r.A.X, Y: 0})
if wall_bottom {
return wall_bottom, inside, col4
}
return false, false, p
}
}
func ProjectPolygon(axis Vector2d, p Polygon2d) (min float32, max float32) {
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
i_point := Point2d{}
distance_sq := float32(math.MaxFloat32)
for point, _ := range r.Points {
if point == len(r.Points) {
break
}
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 {
i_point = pt
distance_sq = d_sq
}
}
}
return intersection, i_point
}
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
}
}
// 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 := 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
if p1.Origin.Sub(p2.Origin).Dot(axis) < 0 {
translation = translation.Scale(-1)
}
}
}
return intersect, will_intersect, translation.Scale(minimum_interval_distance)
}