You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
166 lines
3.9 KiB
166 lines
3.9 KiB
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) |
|
}
|
|
|