diff --git a/toolkit/collection/clone_example_test.go b/toolkit/collection/clone_example_test.go index 2208ba6..5721aa1 100644 --- a/toolkit/collection/clone_example_test.go +++ b/toolkit/collection/clone_example_test.go @@ -37,7 +37,7 @@ func ExampleCloneSliceN() { } // 通过将 map 克隆为 2 个新的 map，将会得到一个新的 map result，而 result 和 map 将不会有任何关联，但是如果 map 中的元素是引用类型，那么 result 中的元素将会和 map 中的元素指向同一个地址 -// - result 的结果为 [map[1:1 2:2 3:3] map[1:1 2:2 3:3]] `无序的 Key-Value 对` +// - result 的结果为 [map[1:1 2:2 3:3] map[1:1 2:2 3:3]] `无序的 Key-value 对` // - 示例中的结果将会输出 2 func ExampleCloneMapN() { var m = map[int]int{1: 1, 2: 2, 3: 3} @@ -59,7 +59,7 @@ func ExampleCloneSlices() { } // 通过将多个 map 克隆为 2 个新的 map，将会得到一个新的 map result，而 result 和 map 将不会有任何关联，但是如果 map 中的元素是引用类型，那么 result 中的元素将会和 map 中的元素指向同一个地址 -// - result 的结果为 [map[1:1 2:2 3:3] map[1:1 2:2 3:3]] `无序的 Key-Value 对` +// - result 的结果为 [map[1:1 2:2 3:3] map[1:1 2:2 3:3]] `无序的 Key-value 对` func ExampleCloneMaps() { var m1 = map[int]int{1: 1, 2: 2, 3: 3} var m2 = map[int]int{1: 1, 2: 2, 3: 3} diff --git a/toolkit/geometry/angle.go b/toolkit/geometry/angle.go new file mode 100644 index 0000000..25176c0 --- /dev/null +++ b/toolkit/geometry/angle.go @@ -0,0 +1,15 @@ +package geometry + +import "github.com/kercylan98/minotaur/toolkit/constraints" + +// CalcAngleDifference 计算两个极角之间的最小角度差。 +// 结果在 -180 到 180 度之间，适用于极角、方位角或其他类似场景。 +func CalcAngleDifference[T constraints.Number](a, b T) float64 { + diff := float64(a) - float64(b) + if diff > 180 { + diff -= 360 + } else if diff < -180 { + diff += 360 + } + return diff +} diff --git a/toolkit/geometry/asserts.go b/toolkit/geometry/asserts.go new file mode 100644 index 0000000..faedc26 --- /dev/null +++ b/toolkit/geometry/asserts.go @@ -0,0 +1,48 @@ +package geometry + +// AssertPolygonValid 断言检查多边形是否有效 +func AssertPolygonValid(polygons ...Polygon) { + for _, polygon := range polygons { + AssertPointValid(polygon...) + if len(polygon) < 3 { + panic("polygon must have at least 3 points") + } + } +} + +// AssertLineSegmentValid 断言检查线段是否有效 +func AssertLineSegmentValid(lineSegments ...LineSegment) { + for _, lineSegment := range lineSegments { + AssertPointValid(lineSegment...) + if len(lineSegment) < 2 { + panic("line segment must have at least 2 points") + } + } +} + +// AssertPointValid 断言检查点是否有效 +func AssertPointValid(points ...Point) { + for _, point := range points { + if len(point) != 2 { + panic("point must have 2 coordinates") + } + } +} + +// AssertVector2Valid 断言检查二维向量是否有效 +func AssertVector2Valid(vectors ...Vector2) { + for _, vector := range vectors { + if len(vector) != 2 { + panic("vector must have 2 coordinates") + } + } +} + +// AssertVector3Valid 断言检查三维向量是否有效 +func AssertVector3Valid(vectors ...Vector3) { + for _, vector := range vectors { + if len(vector) != 3 { + panic("vector must have 3 coordinates") + } + } +} diff --git a/toolkit/geometry/direction_2d.go b/toolkit/geometry/direction_2d.go new file mode 100644 index 0000000..12073ee --- /dev/null +++ b/toolkit/geometry/direction_2d.go @@ -0,0 +1,122 @@ +package geometry + +import ( + "github.com/kercylan98/minotaur/toolkit/constraints" + "math" +) + +// Direction2D 二维方向 +type Direction2D = Vector2 + +var ( + direction2DUnknown = NewVector2(0, 0) // 未知 + direction2DUp = NewVector2(0, 1) // 上 + direction2DDown = NewVector2(0, -1) // 下 + direction2DLeft = NewVector2(-1, 0) // 左 + direction2DRight = NewVector2(1, 0) // 右 + direction2DLeftUp = NewVector2(-1, 1) // 左上 + direction2DRightUp = NewVector2(1, 1) // 右上 + direction2DLeftDown = NewVector2(-1, -1) // 左下 + direction2DRightDown = NewVector2(1, -1) // 右下 + + direction2D4 = []Direction2D{direction2DUp, direction2DDown, direction2DLeft, direction2DRight} // 上下左右四个方向的数组 // 上下左右四个方向的数组 + direction2D8 = []Direction2D{ // 上下左右、左上、右上、左下、右下八个方向的数组 + direction2DUp, direction2DDown, direction2DLeft, direction2DRight, + direction2DLeftUp, direction2DRightUp, direction2DLeftDown, direction2DRightDown, + } +) + +// Direction2D4 上下左右四个方向 +func Direction2D4() []Direction2D { + return direction2D4 +} + +// Direction2D8 上下左右、左上、右上、左下、右下八个方向 +func Direction2D8() []Direction2D { + return direction2D8 +} + +// Direction2DUnknown 获取未知方向 +func Direction2DUnknown() Direction2D { + return direction2DUnknown +} + +// Direction2DUp 获取上方向 +func Direction2DUp() Direction2D { + return direction2DUp +} + +// Direction2DDown 获取下方向 +func Direction2DDown() Direction2D { + return direction2DDown +} + +// Direction2DLeft 获取左方向 +func Direction2DLeft() Direction2D { + return direction2DLeft +} + +// Direction2DRight 获取右方向 +func Direction2DRight() Direction2D { + return direction2DRight +} + +// Direction2DLeftUp 获取左上方向 +func Direction2DLeftUp() Direction2D { + return direction2DLeftUp +} + +// Direction2DRightUp 获取右上方向 +func Direction2DRightUp() Direction2D { + return direction2DRightUp +} + +// Direction2DLeftDown 获取左下方向 +func Direction2DLeftDown() Direction2D { + return direction2DLeftDown +} + +// Direction2DRightDown 获取右下方向 +func Direction2DRightDown() Direction2D { + return direction2DRightDown +} + +// CalcOppositionDirection2D 计算二维方向的反方向 +func CalcOppositionDirection2D(direction Direction2D) Direction2D { + switch { + case direction.Equal(direction2DUp): + return direction2DDown + case direction.Equal(direction2DDown): + return direction2DUp + case direction.Equal(direction2DLeft): + return direction2DRight + case direction.Equal(direction2DRight): + return direction2DLeft + case direction.Equal(direction2DLeftUp): + return direction2DRightDown + case direction.Equal(direction2DRightUp): + return direction2DLeftDown + case direction.Equal(direction2DLeftDown): + return direction2DRightUp + case direction.Equal(direction2DRightDown): + return direction2DLeftUp + default: + return direction2DUnknown + } +} + +// CalcOffsetInDirection2D 计算特定方向上按照指定距离偏移后的坐标 +func CalcOffsetInDirection2D[T constraints.Number](vector Vector2, direction Direction2D, offset T) Vector2 { + return vector.Add(direction.Mul(float64(offset))) +} + +// CalcDirection2DWithAngle 通过角度计算二维方向 +func CalcDirection2DWithAngle[T constraints.Number](angle T) Direction2D { + angleFloat := float64(angle) + return NewVector2(math.Cos(angleFloat), math.Sin(angleFloat)) +} + +// CalcAngleWithDirection2D 计算二维方向的角度 +func CalcAngleWithDirection2D(direction Direction2D) float64 { + return math.Atan2(direction.GetY(), direction.GetX()) +} diff --git a/toolkit/geometry/floor_plan.go b/toolkit/geometry/floor_plan.go new file mode 100644 index 0000000..855c061 --- /dev/null +++ b/toolkit/geometry/floor_plan.go @@ -0,0 +1,38 @@ +package geometry + +import ( + "strings" +) + +// FloorPlan 平面图 +type FloorPlan []string + +// IsFree 检查位置是否为空格 +func (fp FloorPlan) IsFree(position Vector2) bool { + return fp.IsInBounds(position) && fp[int(position.GetY())][int(position.GetX())] == ' ' +} + +// IsInBounds 检查位置是否在边界内 +func (fp FloorPlan) IsInBounds(position Vector2) bool { + x, y := int(position.GetX()), int(position.GetY()) + return (0 <= x && x < len(fp[y])) && (0 <= y && y < len(fp)) +} + +// Put 设置平面图特定位置的字符 +func (fp FloorPlan) Put(position Vector2, c rune) { + x, y := int(position.GetX()), int(position.GetY()) + fp[y] = fp[y][:x] + string(c) + fp[y][x+1:] +} + +// String 获取平面图结果 +func (fp FloorPlan) String() string { + var builder strings.Builder + var last = len(fp) - 1 + for i, row := range fp { + builder.WriteString(row) + if i != last { + builder.WriteByte('\n') + } + } + return builder.String() +} diff --git a/toolkit/geometry/line.go b/toolkit/geometry/line.go index 7fe7580..0a5f27d 100644 --- a/toolkit/geometry/line.go +++ b/toolkit/geometry/line.go @@ -1,82 +1,55 @@ package geometry -import "math" - -type Line [2]Vector2 - // NewLine 创建一条直线 -func NewLine(start, end Vector2) Line { - if len(start) != 2 || len(end) != 2 { - panic("vector size mismatch") - } - return Line{start, end} +func NewLine(point Point, slope float64) Line { + AssertPointValid(point) + return Line{point, slope} } -// GetStart 获取起点 -func (l Line) GetStart() Vector2 { - return l[0] +// Line 直线是由一个点和斜率定义的 +type Line struct { + point Point // 直线上的一个点 + slope float64 // 斜率 } -// GetEnd 获取终点 -func (l Line) GetEnd() Vector2 { - return l[1] +// IsPointOn 判断点是否在直线上 +func (l Line) IsPointOn(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) == point[1]-l.point[1] } -// GetLength 获取长度 -func (l Line) GetLength() float64 { - return l.GetStart().Sub(l.GetEnd()).Length() +// IsPointOnOrAbove 判断点是否在直线上或者在直线上方 +func (l Line) IsPointOnOrAbove(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) >= point[1]-l.point[1] } -// Contains 判断点是否在直线上 -func (l Line) Contains(point Vector2) bool { - if len(point) != 2 { - panic("vector size mismatch") - } - - v1 := l.GetEnd().Sub(l.GetStart()) // 直线的向量 - v2 := point.Sub(l.GetStart()) // 直线起点到给定点的向量 - - // 计算交叉积 - crossProduct := v1[0]*v2[1] - v1[1]*v2[0] - - // 数值误差 - epsilon := 1e-9 - return math.Abs(crossProduct) < epsilon +// IsPointOnOrBelow 判断点是否在直线上或者在直线下方 +func (l Line) IsPointOnOrBelow(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) <= point[1]-l.point[1] } -// Intersect 判断两条直线是否相交 -func (l Line) Intersect(l2 Line) bool { - // 两条直线的向量 - v1 := l.GetEnd().Sub(l.GetStart()) - v2 := l2.GetEnd().Sub(l2.GetStart()) - - // 计算交叉积 - crossProduct1 := v1[0]*v2[1] - v1[1]*v2[0] - - // 两条直线的起点到另一条直线起点的向量 - v3 := l2.GetStart().Sub(l.GetStart()) - v4 := l2.GetEnd().Sub(l.GetStart()) - - // 计算交叉积 - crossProduct2 := v1[0]*v3[1] - v1[1]*v3[0] - crossProduct3 := v1[0]*v4[1] - v1[1]*v4[0] - - return crossProduct1 != 0 && crossProduct2*crossProduct3 < 0 +// IsPointAbove 判断点是否在直线上方 +func (l Line) IsPointAbove(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) > point[1]-l.point[1] } -// IntersectCircle 判断直线是否与圆相交 -func (l Line) IntersectCircle(c Circle) bool { - // 直线的向量 - v1 := l.GetEnd().Sub(l.GetStart()) - - // 直线起点到圆心的向量 - v2 := c.GetCenter().Sub(l.GetStart()) - - // 计算直线到圆心的投影长度 - projection := v1.Dot(v2) / v1.Length() +// IsPointBelow 判断点是否在直线下方 +func (l Line) IsPointBelow(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) < point[1]-l.point[1] +} - // 计算直线到圆心的距离 - distance := v2.Length() +// IsPointLeft 判断点是否在直线左侧 +func (l Line) IsPointLeft(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) < point[1]-l.point[1] +} - return distance <= c.GetRadius() && projection >= 0 && projection <= v1.Length() +// IsPointRight 判断点是否在直线右侧 +func (l Line) IsPointRight(point Point) bool { + AssertPointValid(l.point, point) + return l.slope*(point[0]-l.point[0]) > point[1]-l.point[1] } diff --git a/toolkit/geometry/line_segment.go b/toolkit/geometry/line_segment.go new file mode 100644 index 0000000..5534d64 --- /dev/null +++ b/toolkit/geometry/line_segment.go @@ -0,0 +1,144 @@ +package geometry + +import ( + "github.com/kercylan98/minotaur/toolkit/constraints" + "github.com/kercylan98/minotaur/toolkit/maths" + "math" + "sort" +) + +// NewLineSegment 创建一个线段 +func NewLineSegment(points ...Point) LineSegment { + AssertLineSegmentValid(points) + return points +} + +// LineSegment 由至少两个点组成的线段 +type LineSegment []Point + +// GetLength 获取线段的长度 +func (l LineSegment) GetLength() float64 { + var length float64 + for i := 0; i < len(l)-1; i++ { + length += l[i].Distance2D(l[i+1]) + } + return length +} + +// GetDirection 获取线段的方向 +func (l LineSegment) GetDirection() Vector2 { + return l[0].Sub(l[1]).Normalize() +} + +// GetMidpoint 获取线段的中点 +func (l LineSegment) GetMidpoint() Point { + return l[0].Add(l[1]).Div(2) +} + +// IsPointOnSegment 判断一个给定的点是否在该线段上，允许一定的误差来包容浮点数计算的误差，但不允许点超出线段的范围 +func (l LineSegment) IsPointOnSegment(point Point) bool { + d1 := point.Distance2D(l[0]) + d2 := point.Distance2D(l[1]) + length := l.GetLength() + + // 确保距离之和接近线段长度 + isClose := math.Abs((d1+d2)-length) < 1e-9 + + if !isClose { + return false + } + + // 确保点的坐标在两个端点的范围内 + inXRange := (point.GetX() >= math.Min(l[0].GetX(), l[1].GetX())) && (point.GetX() <= math.Max(l[0].GetX(), l[1].GetX())) + inYRange := (point.GetY() >= math.Min(l[0].GetY(), l[1].GetY())) && (point.GetY() <= math.Max(l[0].GetY(), l[1].GetY())) + + return inXRange && inYRange +} + +// ClosestPoint 计算一个点到该线段的最近的点 +func (l LineSegment) ClosestPoint(point Point) Point { + ax, ay := l[0].GetXY() + bx, by := l[1].GetXY() + ds := l[0].DistanceSquared2D(l[1]) + px, py := point.GetXY() + clamp := maths.Clamp((px-ax)*(bx-ax)+(py-ay)*(by-ay)/ds, 0, 1) + return NewPoint(ax+clamp*(bx-ax), ay+clamp*(by-ay)) +} + +// CalcLineSegmentPointProjection 计算点在线段上的投影 +// - 根据向量的投影公式计算点在线段上的投影，然后将点投影到线段上，得到投影点 +// - 参考：https://en.wikipedia.org/wiki/Vector_projection +func CalcLineSegmentPointProjection(line LineSegment, point Point) Point { + lineDir := line.GetDirection() + pointDir := line[0].Sub(point) + pointProjection := lineDir.Mul(pointDir.Dot(lineDir)) + return line[0].Sub(pointProjection) +} + +// CalcLineSegmentDistanceToPoint 计算点到线段的距禂 +// - 如果点在线段上，则距离为 0 +// - 如果点在线段的延长线上，则距离为点到线段两个端点的最小距离 +// - 否则，计算点到线段的投影点，然后计算点到投影点的距离 +// - 参考：https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line +func CalcLineSegmentDistanceToPoint(line LineSegment, point Point) float64 { + if line.IsPointOnSegment(point) { + return 0 + } + + projection := CalcLineSegmentPointProjection(line, point) + if line.IsPointOnSegment(projection) { + return point.Distance2D(projection) + } + + return math.Min(point.Distance2D(line[0]), point.Distance2D(line[1])) +} + +// CalcLineSegmentPointProjectionDistance 计算点到线段的投影点以及点到投影点的距离 +func CalcLineSegmentPointProjectionDistance(line LineSegment, point Point) (projection Point, distance float64) { + projection = CalcLineSegmentPointProjection(line, point) + distance = point.Distance2D(projection) + return +} + +// CalcLineSegmentCollinearWithEpsilon 计算两条线段是否共线，允许一定的误差来包容浮点数计算的误差 +func CalcLineSegmentCollinearWithEpsilon[T constraints.Number](line1, line2 LineSegment, epsilon T) bool { + area1 := CalcTriangleAreaTwice(line1[0], line1[1], line2[0]) + area2 := CalcTriangleAreaTwice(line1[0], line1[1], line2[1]) + e := float64(epsilon) + return math.Abs(area1-0) <= e && math.Abs(area2-0) <= e +} + +// CalcLineSegmentOverlap 通过对点进行排序来检查两条共线线段是否重叠，返回重叠线段 +func CalcLineSegmentOverlap(line1, line2 LineSegment) (overlap LineSegment, isOverlap bool) { + type PointData struct { + Point + state bool + } + + l1ps, l1pe := PointData{Point: NewPoint(line1[0].GetXY()), state: true}, PointData{Point: NewPoint(line1[1].GetXY()), state: true} + l2ps, l2pe := PointData{Point: NewPoint(line2[0].GetXY()), state: false}, PointData{Point: NewPoint(line2[1].GetXY()), state: false} + + var shapes = [][]PointData{ + {l1ps, l1pe, l1ps}, + {l1ps, l1pe, l1pe}, + {l2ps, l2pe, l2ps}, + {l2ps, l2pe, l2pe}, + } + sort.Slice(shapes, func(i, j int) bool { + a, b := shapes[i], shapes[j] + if a[2].GetX() < b[2].GetX() { + return true + } else if a[2].GetX() > b[2].GetX() { + return false + } else { + return a[2].GetY() < b[2].GetY() + } + }) + + notOverlap := shapes[1][0].state == shapes[2][0].state + singlePointOverlap := shapes[1][2].Equal(shapes[2][2].Point) + if notOverlap || singlePointOverlap { + return overlap, false + } + return NewLineSegment(shapes[1][2].Point, shapes[2][2].Point), true +} diff --git a/toolkit/geometry/point.go b/toolkit/geometry/point.go new file mode 100644 index 0000000..a26b851 --- /dev/null +++ b/toolkit/geometry/point.go @@ -0,0 +1,19 @@ +package geometry + +import "github.com/kercylan98/minotaur/toolkit/constraints" + +// NewPoint 创建一个点 +func NewPoint[V constraints.Number](x, y V) Point { + return NewVector2(x, y) +} + +// NewPosition 创建一个位置 +func NewPosition[V constraints.Number](x, y V) Position { + return NewVector2(x, y) +} + +// Point 由二维向量组成的点 +type Point = Vector2 + +// Position 位置，等同于 Point +type Position = Point diff --git a/toolkit/geometry/polygon.go b/toolkit/geometry/polygon.go new file mode 100644 index 0000000..f8b2ae6 --- /dev/null +++ b/toolkit/geometry/polygon.go @@ -0,0 +1,166 @@ +package geometry + +import "math" + +// NewPolygon 创建一个多边形 +func NewPolygon(points ...Point) Polygon { + AssertPolygonValid(points) + return points +} + +// Polygon 由至少三个点组成的多边形 +type Polygon []Point + +// GetEdges 获取多边形的边 +func (p Polygon) GetEdges() []LineSegment { + AssertPolygonValid(p) + var edges []LineSegment + for i := 0; i < len(p); i++ { + edges = append(edges, NewLineSegment(p[i], p[(i+1)%len(p)])) + } + return edges +} + +// IsPointInside 判断点是否在多边形内 +func (p Polygon) IsPointInside(point Point) bool { + x, y := point.GetXY() + inside := false + for i, j := 0, len(p)-1; i < len(p); i, j = i+1, i { + ix := p[i].GetX() + iy := p[i].GetY() + jx := p[j].GetX() + jy := p[j].GetY() + + if ((iy <= y && y < jy) || (jy <= y && y < iy)) && x < ((jx-ix)*(y-iy))/(jy-iy)+ix { + inside = !inside + } + } + return inside +} + +// IsPointOnEdge 判断点是否在多边形的边上 +func (p Polygon) IsPointOnEdge(point Point) bool { + AssertPolygonValid(p) + for _, edge := range p.GetEdges() { + if edge.IsPointOnSegment(point) { + return true + } + } + return false +} + +// CircumscribedCircleCenter 计算多边形外接圆的圆心 +func (p Polygon) CircumscribedCircleCenter() Point { + return CalcPolygonCircumscribedCircleCenter(p) +} + +// CircumscribedCircleRadius 计算多边形外接圆的半径 +func (p Polygon) CircumscribedCircleRadius() float64 { + return CalcPolygonCircumscribedCircleRadius(p) +} + +// CircumscribedCircleRadiusWithVerticesCentroid 基于多边形的顶点平均值计算的质心计算多边形外接圆的半径 +func (p Polygon) CircumscribedCircleRadiusWithVerticesCentroid() float64 { + var boundingRadius float64 + var centroid = CalcRectangleVerticesCentroid(p) + for _, point := range p { + distance := centroid.Distance2D(point) + if distance > boundingRadius { + boundingRadius = distance + } + } + return boundingRadius +} + +// VerticesCentroid 基于多边形的顶点的平均值计算质心 +func (p Polygon) VerticesCentroid() Point { + return CalcPolygonVerticesCentroid(p) +} + +// Centroid 计算多边形质心 +func (p Polygon) Centroid() Point { + return CalcPolygonCentroid(p) +} + +// CalcRectangleVerticesCentroid 基于矩形的顶点的平均值计算质心 +func CalcRectangleVerticesCentroid(rectangle Polygon) Point { + var x, y float64 + length := float64(len(rectangle)) + for _, point := range rectangle { + x += point.GetX() + y += point.GetY() + } + x /= length + y /= length + return NewPoint(x, x) +} + +// CalcPolygonVerticesCentroid 基于多边形的顶点的平均值计算质心 +func CalcPolygonVerticesCentroid(polygon Polygon) Point { + var centroid = NewPoint(0, 0) + for _, point := range polygon { + centroid = centroid.Add(point) + } + centroid = centroid.Div(float64(len(polygon))) + return centroid +} + +// CalcPolygonCentroid 计算多边形质心 +func CalcPolygonCentroid(polygon Polygon) Point { + var area float64 + var centroid = NewPoint(0, 0) + for i := 0; i < len(polygon); i++ { + j := (i + 1) % len(polygon) + area += polygon[i][0]*polygon[j][1] - polygon[j][0]*polygon[i][1] + centroid = centroid.Add(polygon[i].Add(polygon[j]).Mul(polygon[i][0]*polygon[j][1] - polygon[j][0]*polygon[i][1])) + } + area /= 2 + centroid = centroid.Div(6 * area) + return centroid +} + +// CalcPolygonCircumscribedCircleCenter 计算多边形外接圆的圆心 +func CalcPolygonCircumscribedCircleCenter(polygon Polygon) Point { + // 计算多边形的中心 + var center = NewPoint(0, 0) + for _, point := range polygon { + center = center.Add(point) + } + center = center.Div(float64(len(polygon))) + + // 计算多边形的外接圆半径 + var radius float64 + for _, point := range polygon { + radius = math.Max(radius, point.Distance2D(center)) + } + + return center +} + +// CalcPolygonCircumscribedCircleRadius 计算多边形外接圆的半径 +func CalcPolygonCircumscribedCircleRadius(polygon Polygon) float64 { + center := CalcPolygonCircumscribedCircleCenter(polygon) + var radius float64 + for _, point := range polygon { + radius = math.Max(radius, point.Distance2D(center)) + } + return radius +} + +// CalcPolygonPointProjection 给定一个点和一个多边形，计算多边形边界上与该点距离最短的点，并返回投影点和距离 +func CalcPolygonPointProjection(polygon Polygon, point Point) (projection Point, distance float64) { + var closestProjection Point + var hasClosestProjection bool + var closestDistance float64 + for _, edge := range polygon.GetEdges() { + projectedPoint := edge.ClosestPoint(point) + distance := point.Distance2D(projectedPoint) + if !hasClosestProjection || distance < closestDistance { + closestDistance = distance + closestProjection = projectedPoint + hasClosestProjection = true + } + } + + return closestProjection, closestDistance +} diff --git a/toolkit/geometry/triangle.go b/toolkit/geometry/triangle.go new file mode 100644 index 0000000..af55030 --- /dev/null +++ b/toolkit/geometry/triangle.go @@ -0,0 +1,10 @@ +package geometry + +// CalcTriangleAreaTwice 计算三角形面积的两倍 +func CalcTriangleAreaTwice(a, b, c Vector2) float64 { + ax := b.GetX() - a.GetX() + ay := b.GetY() - a.GetY() + bx := c.GetX() - a.GetX() + by := c.GetY() - a.GetY() + return bx*ay - ax*by +} diff --git a/toolkit/geometry/vector.go b/toolkit/geometry/vector.go index 0e9b73b..cbdd9d2 100644 --- a/toolkit/geometry/vector.go +++ b/toolkit/geometry/vector.go @@ -2,6 +2,7 @@ package geometry import ( "github.com/kercylan98/minotaur/toolkit/constraints" + "github.com/kercylan98/minotaur/toolkit/ident" "math" ) @@ -67,6 +68,16 @@ func NewVector[T constraints.Number](v ...T) Vector { return vec } +// NewVector2 创建一个二维向量 +func NewVector2[T constraints.Number](x, y T) Vector2 { + return NewVector(x, y) +} + +// NewVector3 创建一个三维向量 +func NewVector3[T constraints.Number](x, y, z T) Vector3 { + return NewVector(x, y, z) +} + // Add 向量相加 func (v Vector) Add(v2 Vector) Vector { if len(v) != len(v2) { @@ -121,11 +132,12 @@ func (v Vector) Dot(v2 Vector) float64 { return result } -// Cross 对三维向量进行叉乘 -func (v Vector) Cross(v2 Vector3) Vector { +// Cross3D 对三维向量进行叉乘 +func (v Vector) Cross3D(v2 Vector3) Vector { if len(v) != 3 || len(v2) != 3 { panic("vector size mismatch") } + return NewVector( v[1]*v2[2]-v[2]*v2[1], v[2]*v2[0]-v[0]*v2[2], @@ -133,6 +145,15 @@ func (v Vector) Cross(v2 Vector3) Vector { ) } +// Cross2D 对二维向量进行叉乘 +func (v Vector2) Cross2D(v2 Vector2) float64 { + if len(v) != 2 || len(v2) != 2 { + panic("vector size mismatch") + } + + return v[0]*v2[1] - v[1]*v2[0] +} + // Length 向量长度 func (v Vector) Length() float64 { return math.Sqrt(v.Dot(v)) @@ -148,6 +169,11 @@ func (v Vector) Angle(v2 Vector) float64 { return math.Acos(v.Dot(v2) / (v.Length() * v2.Length())) } +// PolarAngle 极坐标角度，即点在极坐标系中的角度 +func (v Vector2) PolarAngle(v2 Vector2) float64 { + return math.Atan2(v2.GetY()-v.GetY(), v2.GetX()-v.GetX()) +} + // Equal 判断两个向量是否相等 func (v Vector) Equal(v2 Vector) bool { if len(v) != len(v2) { @@ -178,9 +204,14 @@ func (v Vector) IsZero() bool { return true } +// Key 返回向量的键值 +func (v Vector) Key() string { + return ident.GenerateOrderedUniqueIdentStringWithUInt64() +} + // IsParallel 判断两个向量是否平行 -func (v Vector) IsParallel(v2 Vector3) bool { - return v.Cross(v2).IsZero() +func (v Vector3) IsParallel(v2 Vector3) bool { + return v.Cross3D(v2).IsZero() } // IsOrthogonal 判断两个向量是否垂直 @@ -225,9 +256,7 @@ func (v Vector3) GetZ() float64 { // Quadrant 获取向量所在象限 func (v Vector2) Quadrant() int { - if len(v) != 2 { - panic("vector size mismatch") - } + AssertVector2Valid(v) x, y := v.GetXY() switch { case x > 0 && y > 0: @@ -243,6 +272,21 @@ func (v Vector2) Quadrant() int { } } +// Distance2D 计算二维向量之间的距离 +func (v Vector2) Distance2D(v2 Vector2) float64 { + x1, y1 := v.GetXY() + x2, y2 := v2.GetXY() + return math.Sqrt(math.Pow(x2-x1, 2) + math.Pow(y2-y1, 2)) +} + +// DistanceSquared2D 计算二维向量之间的距离的平方 +// - 用于比较距离，避免开方运算 +func (v Vector2) DistanceSquared2D(v2 Vector2) float64 { + x1, y1 := v.GetXY() + x2, y2 := v2.GetXY() + return math.Pow(x2-x1, 2) + math.Pow(y2-y1, 2) +} + // GetXY 返回该点的 x、y 坐标 func (v Vector2) GetXY() (x, y float64) { return v.GetX(), v.GetY() diff --git a/toolkit/maths/maths.go b/toolkit/maths/maths.go index 29307d8..e52c095 100644 --- a/toolkit/maths/maths.go +++ b/toolkit/maths/maths.go @@ -9,3 +9,30 @@ import ( func Sqrt[T constraints.Number](x T) float64 { return math.Sqrt(float64(x)) } + +// MinMax 传入任意数值类型，返回其最小值和最大值 +func MinMax[T constraints.Number](a, b T) (min, max T) { + if a < b { + return a, b + } + return b, a +} + +// MaxMin 传入任意数值类型，返回其最大值和最小值 +func MaxMin[T constraints.Number](a, b T) (max, min T) { + if a < b { + return b, a + } + return a, b +} + +// Clamp 将给定值限制在最小值和最大值之间 +func Clamp[T constraints.Number](value, min, max T) T { + if value < min { + return min + } + if value > max { + return max + } + return value +} diff --git a/toolkit/navigate/astar/astar.go b/toolkit/navigate/astar/astar.go new file mode 100644 index 0000000..bfefb38 --- /dev/null +++ b/toolkit/navigate/astar/astar.go @@ -0,0 +1,56 @@ +package astar + +import ( + "container/heap" + "github.com/kercylan98/minotaur/toolkit/constraints" +) + +// Find 使用 A* 算法在导航网格上查找从起点到终点的最短路径，并返回路径上的节点序列。 +// +// 参数： +// - graph: 图对象，类型为 Graph[Node]，表示导航网格。 +// - start: 起点节点，类型为 Node，表示路径的起点。 +// - end: 终点节点，类型为 Node，表示路径的终点。 +// - cost: 路径代价函数，类型为 func(a, b Node) V，用于计算两个节点之间的代价。 +// - heuristic: 启发函数，类型为 func(a, b Node) V，用于估计从当前节点到目标节点的启发式代价。 +// +// 返回值： +// - []Node: 节点序列，表示从起点到终点的最短路径。如果找不到路径，则返回空序列。 +// +// 注意事项： +// - graph 对象表示导航网格，其中包含了节点和连接节点的边。 +// - start 和 end 分别表示路径的起点和终点。 +// - cost 函数用于计算两个节点之间的代价，可以根据实际情况自定义实现。 +// - heuristic 函数用于估计从当前节点到目标节点的启发式代价，可以根据实际情况自定义实现。 +// - 函数使用了 A* 算法来搜索最短路径。 +// - 函数内部使用了堆数据结构来管理待处理的节点。 +// - 函数返回一个节点序列，表示从起点到终点的最短路径。如果找不到路径，则返回空序列。 +func Find[I constraints.Ordered, T any](graph Graph[I, T], start, end T, cost, heuristic func(a, b T) float64) []T { + closed := make(map[I]bool) + + h := &heapQueue[path[T]]{} + heap.Init(h) + heap.Push(h, &heapItem[path[T]]{value: path[T]{start}}) + + for h.Len() > 0 { + p := heap.Pop(h).(*heapItem[path[T]]).value + n := p.Last() + if closed[graph.GetNodeId(n)] { + continue + } + if graph.GetNodeId(n) == graph.GetNodeId(end) { + return p + } + closed[graph.GetNodeId(n)] = true + + for _, nb := range graph.GetNeighbours(n) { + cp := p.Extend(nb) + heap.Push(h, &heapItem[path[T]]{ + value: cp, + priority: -(cp.Cost(cost) + heuristic(nb, end)), + }) + } + } + + return nil +} diff --git a/toolkit/navigate/astar/astar_example_test.go b/toolkit/navigate/astar/astar_example_test.go new file mode 100644 index 0000000..c420dcb --- /dev/null +++ b/toolkit/navigate/astar/astar_example_test.go @@ -0,0 +1,79 @@ +package astar_test + +import ( + "fmt" + "github.com/kercylan98/minotaur/toolkit/geometry" + "github.com/kercylan98/minotaur/toolkit/navigate/astar" +) + +type Graph struct { + geometry.FloorPlan +} + +type Entity struct { + geometry.Vector2 +} + +func (g *Graph) GetNodeId(node *Entity) int { + x, y := node.GetXY() + a, b := int(x), int(y) + mergedNumber := (a << 16) | (b & 0xFFFFFFFF) + return mergedNumber +} + +func (g *Graph) GetNeighbours(t *Entity) []*Entity { + var neighbours []*Entity + for _, direction := range geometry.Direction2D4() { + next := geometry.CalcOffsetInDirection2D(t.Vector2, direction, 1) + if g.FloorPlan.IsFree(next) { + neighbours = append(neighbours, &Entity{ + Vector2: next, + }) + } + } + return neighbours +} + +func ExampleFind() { + graph := Graph{ + FloorPlan: geometry.FloorPlan{ + "===========", + "X XX X X", + "X X XX X", + "X XX X", + "X XXX X", + "X XX X X", + "X XX X X", + "===========", + }, + } + + paths := astar.Find[int, *Entity]( + &graph, + &Entity{geometry.NewVector2(1, 1)}, + &Entity{geometry.NewVector2(8, 6)}, + // 曼哈顿距离 + func(a, b *Entity) float64 { + return a.Sub(b.Vector2).Length() + }, + func(a, b *Entity) float64 { + return a.Sub(b.Vector2).Length() + }, + ) + + for _, path := range paths { + graph.Put(path.Vector2, '.') + } + + fmt.Println(graph) + + // Output: + // =========== + // X.XX X X + // X. X XX X + // X.XX .....X + // X.....XXX.X + // X XX X .X + // X XX X ..X + // =========== +} diff --git a/toolkit/navigate/astar/graph.go b/toolkit/navigate/astar/graph.go new file mode 100644 index 0000000..c412f89 --- /dev/null +++ b/toolkit/navigate/astar/graph.go @@ -0,0 +1,11 @@ +package astar + +import "github.com/kercylan98/minotaur/toolkit/constraints" + +// Graph 适用于 A* 算法的图数据结构接口定义，表示导航网格，其中包含了节点和连接节点的边。 +type Graph[I constraints.Ordered, T any] interface { + // GetNodeId 返回节点的唯一标识。 + GetNodeId(node T) I + // GetNeighbours 返回与给定节点相邻的节点列表。 + GetNeighbours(t T) []T +} diff --git a/toolkit/navigate/astar/path.go b/toolkit/navigate/astar/path.go new file mode 100644 index 0000000..f751145 --- /dev/null +++ b/toolkit/navigate/astar/path.go @@ -0,0 +1,29 @@ +package astar + +// path 表示一条路径，是一系列节点的有序序列。 +type path[T any] []T + +// Last 获取路径中的最后一个节点 +func (p path[T]) Last() T { + if len(p) == 0 { + panic("empty path") + } + return p[len(p)-1] +} + +// Extend 通过追加一个节点创建一个新的路径 +// 这个方法返回一个新的路径，而不会改变原始路径 +func (p path[T]) Extend(n T) path[T] { + newPath := append(make(path[T], 0, len(p)+1), p...) + return append(newPath, n) +} + +// Cost 计算路径的总成本 +// 参数 `costFunc` 是一个函数，计算两个节点之间的成本 +func (p path[T]) Cost(f func(a, b T) float64) float64 { + totalCost := 0.0 + for i := 1; i < len(p); i++ { + totalCost += f(p[i-1], p[i]) + } + return totalCost +} diff --git a/toolkit/navigate/astar/priority.go b/toolkit/navigate/astar/priority.go new file mode 100644 index 0000000..e845ec1 --- /dev/null +++ b/toolkit/navigate/astar/priority.go @@ -0,0 +1,41 @@ +package astar + +// heapItem 表示一个带有优先级的项目 +type heapItem[T any] struct { + value T + priority float64 +} + +// heapQueue 是一个基于堆实现的优先级队列 +type heapQueue[T any] []*heapItem[T] + +// Len 返回优先级队列的长度 +func (pq *heapQueue[T]) Len() int { + return len(*pq) +} + +// Less 比较两个项目的优先级 +// 如果返回 true，表示第一个项目的优先级高于第二个项目 +func (pq *heapQueue[T]) Less(i, j int) bool { + return (*pq)[i].priority > (*pq)[j].priority +} + +// Swap 交换两个项目的位置 +func (pq *heapQueue[T]) Swap(i, j int) { + (*pq)[i], (*pq)[j] = (*pq)[j], (*pq)[i] +} + +// Push 向优先级队列添加一个新项目 +func (pq *heapQueue[T]) Push(x any) { + item := x.(*heapItem[T]) + *pq = append(*pq, item) +} + +// Pop 从优先级队列中移除并返回优先级最高的项目 +func (pq *heapQueue[T]) Pop() any { + old := *pq + n := len(old) + item := old[n-1] + *pq = old[0 : n-1] + return item +} diff --git a/toolkit/navigate/navmesh/funnel.go b/toolkit/navigate/navmesh/funnel.go new file mode 100644 index 0000000..5d40650 --- /dev/null +++ b/toolkit/navigate/navmesh/funnel.go @@ -0,0 +1,82 @@ +package navmesh + +import ( + "github.com/kercylan98/minotaur/toolkit/geometry" +) + +type funnel struct { + path []geometry.Vector2 + portals [][2]geometry.Vector2 +} + +func (slf *funnel) push(point1, point2 geometry.Vector2) { + slf.portals = append(slf.portals, [2]geometry.Vector2{point1, point2}) +} + +func (slf *funnel) pushSingle(point geometry.Vector2) { + slf.portals = append(slf.portals, [2]geometry.Vector2{point, point}) +} + +func (slf *funnel) stringPull() []geometry.Vector2 { + var ( + portals = slf.portals + points []geometry.Vector2 + apexIndex = 0 + leftIndex = 0 + rightIndex = 0 + portalApex = portals[0][0] + portalLeft = portals[0][0] + portalRight = portals[0][1] + ) + + points = append(points, portalApex) + for i := 1; i < len(portals); i++ { + lr := portals[i] + left, right := lr[0], lr[1] + + if geometry.CalcTriangleAreaTwice(portalApex, portalRight, right) <= 0 { + if portalApex.Equal(portalRight) || geometry.CalcTriangleAreaTwice(portalApex, portalLeft, right) > 0 { + portalRight = right + rightIndex = i + } else { + points = append(points, portalLeft) + portalApex = portalLeft + apexIndex = leftIndex + + portalLeft = portalApex + portalRight = portalApex + leftIndex = apexIndex + rightIndex = apexIndex + + i = apexIndex + continue + } + } + + if geometry.CalcTriangleAreaTwice(portalApex, portalLeft, left) >= 0 { + if portalApex.Equal(portalLeft) || geometry.CalcTriangleAreaTwice(portalApex, portalRight, left) < 0 { + portalLeft = left + leftIndex = i + } else { + points = append(points, portalRight) + portalApex = portalRight + apexIndex = rightIndex + + portalLeft = portalApex + portalRight = portalApex + leftIndex = apexIndex + rightIndex = apexIndex + + i = apexIndex + continue + } + } + } + + if len(points) == 0 || !points[len(points)-1].Equal(portals[len(portals)-1][0]) { + points = append(points, portals[len(portals)-1][0]) + } + + slf.path = points + return slf.path +} diff --git a/toolkit/navigate/navmesh/navmesh.go b/toolkit/navigate/navmesh/navmesh.go new file mode 100644 index 0000000..01d023b --- /dev/null +++ b/toolkit/navigate/navmesh/navmesh.go @@ -0,0 +1,277 @@ +package navmesh + +import ( + "github.com/kercylan98/minotaur/toolkit/geometry" + "github.com/kercylan98/minotaur/toolkit/navigate/astar" +) + +// NewNavMesh 创建一个新的导航网格，并返回一个指向该导航网格的指针。 +// +// 参数： +// - shapes: 形状切片，类型为 []geometry.Shape[V]，表示导航网格中的形状。 +// - meshShrinkAmount: 网格缩小量，类型为 V，表示导航网格的缩小量。 +// +// 返回值： +// - *NavMesh[V]: 指向创建的导航网格的指针。 +// +// 注意事项： +// - 导航网格的形状可以是任何几何形状。 +// - meshShrinkAmount 表示导航网格的缩小量，用于在形状之间创建链接时考虑形状的缩小效果。 +// - 函数内部使用了泛型类型参数 V，可以根据需要指定形状的坐标类型。 +// - 函数返回一个指向创建的导航网格的指针。 +// +// 使用建议： +// - 确保 NavMesh 计算精度的情况下，V 建议使用 float64 类型 +func NewNavMesh(shapes []geometry.Polygon, meshShrinkAmount float64) *NavMesh { + nm := &NavMesh{ + meshShapes: make([]*shape, len(shapes)), + meshShrinkAmount: meshShrinkAmount, + } + for i, shape := range shapes { + nm.meshShapes[i] = newShape(i, shape) + } + nm.generateLink() + return nm +} + +type NavMesh struct { + meshShapes []*shape + meshShrinkAmount float64 +} + +// GetNodeId 实现 astar.Graph 的接口，用于返回给定形状的唯一标识。 +func (m *NavMesh) GetNodeId(node *shape) int { + return node.id +} + +// GetNeighbours 实现 astar.Graph 的接口，用于向 A* 算法提供相邻图形 +func (m *NavMesh) GetNeighbours(node *shape) []*shape { + return node.links +} + +// Find 用于在 NavMesh 中查找离给定点最近的形状，并返回距离、找到的点和找到的形状。 +// +// 参数： +// - point: 给定的点，类型为 geometry.Point，表示一个 V 维度的点坐标。 +// - maxDistance: 最大距离，类型为 V，表示查找的最大距离限制。 +// +// 返回值：当 distance 为 -1 时表示未找到最近的形状，否则返回距离、找到的点和找到的形状。 +// - distance: 距离，类型为 float64，表示离给定点最近的形状的距离。 +// - findPoint: 找到的点，类型为 geometry.Point，表示离给定点最近的点坐标。 +// - findPolygon: 找到的多边形，类型为 geometry.Polygon，表示离给定点最近的形状。 +// +// 注意事项： +// - 如果给定点在 NavMesh 中的某个形状内部或者在形状的边上，距离为 0，找到的形状为该形状，找到的点为给定点。 +// - 如果给定点不在任何形状内部或者形状的边上，将计算给定点到每个形状的距离，并找到最近的形状和对应的点。 +// - 距离的计算采用几何学中的投影点到形状的距离。 +// - 函数返回离给定点最近的形状的距离、找到的点和找到的形状。 +func (m *NavMesh) Find(point geometry.Point, maxDistance float64) (distance float64, findPoint geometry.Point, findPolygon geometry.Polygon) { + var minDistance = maxDistance + var closest *shape + var pointOnClosest geometry.Point + for _, meshShape := range m.meshShapes { + if meshShape.IsPointInside(point) || meshShape.IsPointOnEdge(point) { + minDistance = 0 + closest = meshShape + pointOnClosest = point + break + } + br := meshShape.CircumscribedCircleRadius() + distance := meshShape.VerticesCentroid().Distance2D(point) + if distance-br < minDistance { + point, distance := geometry.CalcPolygonPointProjection(meshShape.Polygon, point) + if distance < minDistance { + minDistance = distance + closest = meshShape + pointOnClosest = point + } + } + } + if closest == nil { + return -1, nil, nil + } + return minDistance, pointOnClosest, closest.Polygon +} + +// FindPath 函数用于在 NavMesh 中查找从起点到终点的路径，并返回路径上的点序列。 +// +// 参数： +// - start: 起点，类型为 geometry.Point[V]，表示路径的起始点。 +// - end: 终点，类型为 geometry.Point[V]，表示路径的终点。 +// +// 返回值： +// - result: 路径上的点序列，类型为 []geometry.Point[V]。 +// +// 注意事项： +// - 函数首先根据起点和终点的位置，找到离它们最近的形状作为起点形状和终点形状。 +// - 如果起点或终点不在任何形状内部，且 NavMesh 的 meshShrinkAmount 大于0，则会考虑缩小的形状。 +// - 使用 A* 算法在 NavMesh 上搜索从起点形状到终点形状的最短路径。 +// - 使用漏斗算法对路径进行优化，以得到最终的路径点序列。 +func (m *NavMesh) FindPath(start, end geometry.Point) (result []geometry.Point) { + var startShape, endShape *shape + var startDistance, endDistance = -1.0, -1.0 + + for _, meshShape := range m.meshShapes { + br := meshShape.boundingRadius + + distance := meshShape.centroid.Distance2D(start) + if (distance <= startDistance || startDistance == -1) && distance <= br && meshShape.IsPointInside(start) { + startShape = meshShape + startDistance = distance + } + + distance = meshShape.centroid.Distance2D(end) + if (distance <= endDistance || endDistance == -1) && distance <= br && meshShape.IsPointInside(end) { + endShape = meshShape + endDistance = distance + } + } + + if endShape == nil && m.meshShrinkAmount > 0 { + for _, meshShape := range m.meshShapes { + br := meshShape.boundingRadius + m.meshShrinkAmount + distance := meshShape.centroid.Distance2D(end) + if distance <= br { + _, projectionDistance := geometry.CalcPolygonPointProjection(meshShape.Polygon, end) + if projectionDistance <= m.meshShrinkAmount && projectionDistance < endDistance { + endShape = meshShape + endDistance = projectionDistance + } + } + } + } + + if endShape == nil { + return + } + + if startShape == nil && m.meshShrinkAmount > 0 { + for _, meshShape := range m.meshShapes { + br := meshShape.BoundingRadius() + m.meshShrinkAmount + distance := meshShape.centroid.Distance2D(start) + if distance <= br { + _, projectionDistance := geometry.CalcPolygonPointProjection(meshShape.Polygon, start) + if projectionDistance <= m.meshShrinkAmount && projectionDistance < startDistance { + startShape = meshShape + startDistance = projectionDistance + } + } + } + } + + if startShape == nil { + return + } + + if startShape == endShape { + return append(result, start, end) + } + + path := astar.Find[int, *shape](m, startShape, endShape, + func(a, b *shape) float64 { + return a.VerticesCentroid().Distance2D(b.VerticesCentroid()) + }, + func(a, b *shape) float64 { + return a.VerticesCentroid().Distance2D(b.VerticesCentroid()) + }, + ) + + if len(path) == 0 { + return + } + + path = append([]*shape{startShape}, path...) + + f := new(funnel) + f.pushSingle(start) + for i := 0; i < len(path)-1; i++ { + current := path[i] + next := path[i+1] + if current.id == next.id { + continue + } + + var portal geometry.LineSegment + var find bool + for i := 0; i < len(current.links); i++ { + if current.links[i].id == next.id { + portal = current.portals[i] + find = true + } + } + if !find { + panic("not found portal") + } + + f.push(portal[0], portal[1]) + } + f.pushSingle(end) + f.stringPull() + + var lastPoint geometry.Point + for i, point := range f.path { + var np = point.Clone() + if i == 0 || !np.Equal(lastPoint) { + result = append(result, np) + } + lastPoint = np + } + return result +} + +func (m *NavMesh) generateLink() { + for i := 0; i < len(m.meshShapes); i++ { + shapePkg := m.meshShapes[i] + shapeCentroid := shapePkg.centroid + shapeBoundingRadius := shapePkg.boundingRadius + shapeEdges := shapePkg.Edges() + for t := i + 1; t < len(m.meshShapes); t++ { + targetShapePkg := m.meshShapes[t] + targetShapeCentroid := targetShapePkg.centroid + targetShapeBoundingRadius := targetShapePkg.boundingRadius + centroidDistance := shapeCentroid.Distance2D(targetShapeCentroid) + if centroidDistance > shapeBoundingRadius+targetShapeBoundingRadius { + continue + } + + for _, shapeEdge := range shapeEdges { + for _, targetEdge := range targetShapePkg.Edges() { + if !geometry.CalcLineSegmentCollinearWithEpsilon(shapeEdge, targetEdge, 1e-4) { + continue + } + + var overlapLine, overlap = geometry.CalcLineSegmentOverlap(shapeEdge, targetEdge) + if !overlap { + continue + } + + shapePkg.links = append(shapePkg.links, targetShapePkg) + targetShapePkg.links = append(targetShapePkg.links, shapePkg) + + edgeAngle := shapeCentroid.PolarAngle(shapeEdge[0]) + a1 := shapeCentroid.PolarAngle(overlapLine[0]) + a2 := shapeCentroid.PolarAngle(overlapLine[1]) + a3 := geometry.CalcAngleDifference(edgeAngle, a1) + a4 := geometry.CalcAngleDifference(edgeAngle, a2) + if a3 < a4 { + shapePkg.portals = append(shapePkg.portals, geometry.NewLineSegment(overlapLine[0], overlapLine[1])) + } else { + shapePkg.portals = append(shapePkg.portals, geometry.NewLineSegment(overlapLine[1], overlapLine[0])) + } + + edgeAngle = targetShapeCentroid.PolarAngle(targetEdge[0]) + a1 = targetShapeCentroid.PolarAngle(overlapLine[0]) + a2 = targetShapeCentroid.PolarAngle(overlapLine[1]) + a3 = geometry.CalcAngleDifference(edgeAngle, a1) + a4 = geometry.CalcAngleDifference(edgeAngle, a2) + if a3 < a4 { + targetShapePkg.portals = append(targetShapePkg.portals, geometry.NewLineSegment(overlapLine[0], overlapLine[1])) + } else { + targetShapePkg.portals = append(targetShapePkg.portals, geometry.NewLineSegment(overlapLine[1], overlapLine[0])) + } + } + } + + } + } +} diff --git a/toolkit/navigate/navmesh/navmesh_example_test.go b/toolkit/navigate/navmesh/navmesh_example_test.go new file mode 100644 index 0000000..1ee469d --- /dev/null +++ b/toolkit/navigate/navmesh/navmesh_example_test.go @@ -0,0 +1,119 @@ +package navmesh_test + +import ( + "fmt" + "github.com/kercylan98/minotaur/toolkit/geometry" + "github.com/kercylan98/minotaur/toolkit/maths" + "github.com/kercylan98/minotaur/toolkit/navigate/navmesh" +) + +func ExampleNavMesh_FindPath() { + fp := geometry.FloorPlan{ + "=================================", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "X X", + "=================================", + } + + var walkable []geometry.Polygon + walkable = append(walkable, + geometry.NewPolygon( + geometry.NewPoint(5, 5), + geometry.NewPoint(15, 5), + geometry.NewPoint(15, 15), + geometry.NewPoint(5, 15), + ), + geometry.NewPolygon( + geometry.NewPoint(15, 5), + geometry.NewPoint(25, 5), + geometry.NewPoint(25, 15), + geometry.NewPoint(15, 15), + ), + geometry.NewPolygon( + geometry.NewPoint(15, 15), + geometry.NewPoint(25, 15), + geometry.NewPoint(25, 25), + geometry.NewPoint(15, 25), + ), + ) + + for _, shape := range walkable { + for _, edge := range shape.GetEdges() { + sx, bx := maths.MinMax(edge[0].GetX(), edge[1].GetX()) + sy, by := maths.MinMax(edge[0].GetY(), edge[1].GetY()) + + for x := sx; x <= bx; x++ { + for y := sy; y <= by; y++ { + fp.Put(geometry.NewPoint(x, y), '+') + } + } + } + } + + nm := navmesh.NewNavMesh(walkable, 0) + path := nm.FindPath( + geometry.NewPoint(6, 6), + geometry.NewPoint(18, 24), + ) + for _, point := range path { + fp.Put(geometry.NewPoint(point.GetX(), point.GetY()), 'G') + } + + fmt.Println(fp) + + // Output: + // ================================= + // X X + // X X + // X X + // X X + // X +++++++++++++++++++++ X + // X +G + + X + // X + + + X + // X + + + X + // X + + + X + // X + + + X + // X + + + X + // X + + + X + // X + + + X + // X + + + X + // X ++++++++++G++++++++++ X + // X + + X + // X + + X + // X + + X + // X + + X + // X + + X + // X + + X + // X + + X + // X + + X + // X + G + X + // X +++++++++++ X + // X X + // X X + // ================================= +} diff --git a/toolkit/navigate/navmesh/shape.go b/toolkit/navigate/navmesh/shape.go new file mode 100644 index 0000000..ee94330 --- /dev/null +++ b/toolkit/navigate/navmesh/shape.go @@ -0,0 +1,52 @@ +package navmesh + +import ( + "github.com/kercylan98/minotaur/toolkit/geometry" +) + +func newShape(id int, s geometry.Polygon) *shape { + return &shape{ + id: id, + Polygon: s, + centroid: geometry.CalcRectangleVerticesCentroid(s), + boundingRadius: s.CircumscribedCircleRadiusWithVerticesCentroid(), + edges: s.GetEdges(), + } +} + +type shape struct { + id int + geometry.Polygon + links []*shape + portals []geometry.LineSegment + boundingRadius float64 + centroid geometry.Point + edges []geometry.LineSegment + + weight float64 + x, y float64 +} + +func (slf *shape) Id() int { + return slf.id +} + +func (slf *shape) Edges() []geometry.LineSegment { + return slf.edges +} + +func (slf *shape) BoundingRadius() float64 { + return slf.boundingRadius +} + +func (slf *shape) Centroid() geometry.Point { + return slf.centroid +} + +func (slf *shape) IsWall() bool { + return slf.weight == 0 +} + +func (slf *shape) GetCost(point geometry.Point) float64 { + return slf.Centroid().Distance2D(point) +}