163 lines
3.5 KiB
Go
163 lines
3.5 KiB
Go
package vector
|
|
|
|
import (
|
|
"math"
|
|
)
|
|
|
|
// A and B represent two opposite corners of axis-aligned boundin box
|
|
type AABB2d struct {
|
|
A Point2d `json:"A"`
|
|
B Point2d `json:"B"`
|
|
}
|
|
|
|
type Polygon2d struct {
|
|
Points []Vector2d `json:"points"`
|
|
Normals []Vector2d `json:"normals"`
|
|
Origin Point2d `json:"origin"`
|
|
}
|
|
|
|
func (p Polygon2d) Edge(i int) (Point2d, Vector2d) {
|
|
p1 := Point2d{}
|
|
p2 := Point2d{}
|
|
|
|
if i == len(p.Points)-1 {
|
|
p1 = p.Origin.Add(p.Points[i])
|
|
p2 = p.Origin.Add(p.Points[0])
|
|
} else {
|
|
p1 = p.Origin.Add(p.Points[i])
|
|
p2 = p.Origin.Add(p.Points[i+1])
|
|
}
|
|
return p1, p2.Sub(p1)
|
|
}
|
|
|
|
type Vector2d struct {
|
|
X float64 `json:"x"`
|
|
Y float64 `json:"y"`
|
|
}
|
|
|
|
type Point2d struct {
|
|
X float64 `json:"x"`
|
|
Y float64 `json:"y"`
|
|
}
|
|
|
|
const Epsilon = 1e-7
|
|
const Rad2deg = 180 / math.Pi
|
|
const Deg2rad = math.Pi / 180
|
|
|
|
func (p1 Point2d) Sub(p2 Point2d) Vector2d {
|
|
return Vector2d{p1.X - p2.X, p1.Y - p2.Y}
|
|
}
|
|
|
|
func (p Point2d) Add(v Vector2d) Point2d {
|
|
return Point2d{p.X + v.X, p.Y + v.Y}
|
|
}
|
|
|
|
func (v Vector2d) Mag() float64 {
|
|
return math.Abs(math.Sqrt(v.X*v.X + v.Y*v.Y))
|
|
}
|
|
|
|
func (v Vector2d) MagSquared() float64 {
|
|
return math.Abs(v.X*v.X + v.Y*v.Y)
|
|
}
|
|
|
|
func (v Vector2d) PopPop() float64 {
|
|
return v.Mag()
|
|
}
|
|
|
|
func (v Vector2d) Scale(s float64) Vector2d {
|
|
return Vector2d{v.X * s, v.Y * s}
|
|
}
|
|
|
|
func (v1 Vector2d) Cross(v2 Vector2d) float64 {
|
|
return v1.X*v2.Y - v1.Y*v2.X
|
|
}
|
|
|
|
func (v1 Vector2d) Dot(v2 Vector2d) float64 {
|
|
return (v1.X * v2.X) + (v1.Y * v2.Y)
|
|
}
|
|
|
|
func (p Point2d) ToVector() Vector2d {
|
|
return Vector2d{p.X, p.Y}
|
|
}
|
|
|
|
func (v Vector2d) ToPoint() Point2d {
|
|
return Point2d{v.X, v.Y}
|
|
}
|
|
|
|
func (v1 Vector2d) Rotate(a float64) Vector2d {
|
|
x := float64(v1.X)*math.Cos(float64(a)) - float64(v1.Y)*math.Sin(float64(a))
|
|
y := float64(v1.X)*math.Sin(float64(a)) + float64(v1.Y)*math.Cos(float64(a))
|
|
return Vector2d{x, y}
|
|
}
|
|
|
|
func PointInRect(p Point2d, r AABB2d) bool {
|
|
return (p.X > r.A.X) && (p.X < r.B.X) && (p.Y > r.A.Y) && (p.Y < r.B.Y)
|
|
}
|
|
|
|
func AASquareAtPoint(p Point2d, edgeLength float64) AABB2d {
|
|
size := edgeLength / 2.0
|
|
return AABB2d{
|
|
A: Point2d{X: p.X - size, Y: p.Y - size},
|
|
B: Point2d{X: p.X + size, Y: p.Y + size}}
|
|
}
|
|
|
|
func (r AABB2d) ToPolygon() Polygon2d {
|
|
p := Polygon2d{}
|
|
|
|
p.Origin = r.A
|
|
p.Points = append(p.Points, r.A.Sub(p.Origin))
|
|
p.Points = append(p.Points, Point2d{r.A.X, r.B.Y}.Sub(p.Origin))
|
|
p.Points = append(p.Points, r.B.Sub(p.Origin))
|
|
p.Points = append(p.Points, Point2d{r.B.X, r.A.Y}.Sub(p.Origin))
|
|
|
|
return p
|
|
}
|
|
|
|
func OrientedSquare(center Point2d, heading Vector2d, size float64) Polygon2d {
|
|
out := Polygon2d{}
|
|
out.Origin.X = center.X
|
|
out.Origin.Y = center.Y
|
|
x := heading.Normalize().Scale(size)
|
|
y := Vector2d{-x.Y, x.X}
|
|
z := Point2d{}
|
|
out.Points = append(out.Points, z.Add(x).Add(y).ToVector())
|
|
out.Points = append(out.Points, z.Add(y).Sub(x.ToPoint()))
|
|
out.Points = append(out.Points, z.Sub(x.ToPoint()).ToPoint().Sub(y.ToPoint()))
|
|
out.Points = append(out.Points, z.Add(x).Sub(y.ToPoint()))
|
|
return out
|
|
}
|
|
|
|
func (v Vector2d) Normalize() Vector2d {
|
|
if v.X == 0 && v.Y == 0 {
|
|
return v
|
|
}
|
|
|
|
m := v.Mag()
|
|
return Vector2d{v.X / m, v.Y / m}
|
|
}
|
|
|
|
func Distance(p1, p2 Point2d) float64 {
|
|
return p1.Sub(p2).Mag()
|
|
}
|
|
|
|
// returns radians
|
|
func Angle(v1, v2 Vector2d) float64 {
|
|
x := (v1.Dot(v2) / (v1.Mag() * v2.Mag()))
|
|
angle := math.Acos(float64(x))
|
|
|
|
if math.IsNaN(angle) {
|
|
if math.Abs(float64(v1.X-v2.X)) > Epsilon {
|
|
return 180.0
|
|
} else {
|
|
return 0
|
|
}
|
|
}
|
|
|
|
// Determine the sign to see what direction
|
|
// the angle should go in
|
|
if v1.Y*v2.X > v1.X*v2.Y {
|
|
angle = angle * -1.0
|
|
}
|
|
return angle
|
|
}
|