2D Vector Library
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 Raw Permalink Blame History

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