三维形状碰撞检测

pkg/alg/aoi.go

@@ -1,11 +1,5 @@
 package alg
 
-import (
-	"math"
-
-	"hk4e/pkg/logger"
-)
-
 // AoiManager aoi管理模块
 type AoiManager struct {
 	// 区域边界坐标
@@ -64,31 +58,6 @@ func (a *AoiManager) Init3DRectAoiManager(numX, numY, numZ int16) {
 	}
 }
 
-func (a *AoiManager) AoiInfoLog(debug bool) {
-	logger.Info("AoiMgr: minX: %d, maxX: %d, numX: %d, minY: %d, maxY: %d, numY: %d, minZ: %d, maxZ: %d, numZ: %d, num grid: %d\n",
-		a.minX, a.maxX, a.numX, a.minY, a.maxY, a.numY, a.minZ, a.maxZ, a.numZ, uint32(a.numX)*uint32(a.numY)*uint32(a.numZ))
-	minGridObjectCount := math.MaxInt32
-	maxGridObjectCount := 0
-	for _, grid := range a.gridMap {
-		gridObjectCount := len(grid.objectMap)
-		if gridObjectCount > maxGridObjectCount {
-			maxGridObjectCount = gridObjectCount
-		}
-		if gridObjectCount < minGridObjectCount {
-			minGridObjectCount = gridObjectCount
-		}
-		if debug {
-			// logger.Debug("Grid: gid: %d, minX: %d, maxX: %d, minY: %d, maxY: %d, minZ: %d, maxZ: %d, object count: %v",
-			// 	grid.gid, grid.minX, grid.maxX, grid.minY, grid.maxY, grid.minZ, grid.maxZ, gridObjectCount)
-			for objectId, object := range grid.objectMap {
-				logger.Debug("objectId: %v, object: %v", objectId, object)
-			}
-		}
-	}
-	logger.Info("min grid object count: %v", minGridObjectCount)
-	logger.Info("max grid object count: %v", maxGridObjectCount)
-}
-
 // GridXLen 每个格子在x轴方向的长度
 func (a *AoiManager) GridXLen() int16 {
 	return (a.maxX - a.minX) / a.numX
@@ -203,7 +172,6 @@ func (a *AoiManager) GetObjectListByPos(x, y, z float32) map[int64]any {
 		for kk, vv := range tmp {
 			objectList[kk] = vv
 		}
-		logger.Debug("Grid: gid: %d, tmp len: %v", v.gid, len(tmp))
 	}
 	return objectList
 }
@@ -212,7 +180,6 @@ func (a *AoiManager) GetObjectListByPos(x, y, z float32) map[int64]any {
 func (a *AoiManager) GetObjectListByGid(gid uint32) map[int64]any {
 	grid := a.gridMap[gid]
 	if grid == nil {
-		logger.Error("grid is nil, gid: %v", gid)
 		return nil
 	}
 	objectList := grid.GetObjectList()
@@ -223,7 +190,6 @@ func (a *AoiManager) GetObjectListByGid(gid uint32) map[int64]any {
 func (a *AoiManager) AddObjectToGrid(objectId int64, object any, gid uint32) {
 	grid := a.gridMap[gid]
 	if grid == nil {
-		logger.Error("grid is nil, gid: %v", gid)
 		return
 	}
 	grid.AddObject(objectId, object)
@@ -233,7 +199,6 @@ func (a *AoiManager) AddObjectToGrid(objectId int64, object any, gid uint32) {
 func (a *AoiManager) RemoveObjectFromGrid(objectId int64, gid uint32) {
 	grid := a.gridMap[gid]
 	if grid == nil {
-		logger.Error("grid is nil, gid: %v", gid)
 		return
 	}
 	grid.RemoveObject(objectId)
@@ -250,3 +215,46 @@ func (a *AoiManager) RemoveObjectFromGridByPos(objectId int64, x, y, z float32)
 	gid := a.GetGidByPos(x, y, z)
 	a.RemoveObjectFromGrid(objectId, gid)
 }
+
+// Grid 地图格子
+type Grid struct {
+	gid uint32 // 格子id
+	// 格子边界坐标
+	// 目前开发阶段暂时用不到 节省点内存
+	// minX      int16
+	// maxX      int16
+	// minY      int16
+	// maxY      int16
+	// minZ      int16
+	// maxZ      int16
+	objectMap map[int64]any // k:objectId v:对象
+}
+
+// NewGrid 初始化格子
+func NewGrid(gid uint32, minX, maxX, minY, maxY, minZ, maxZ int16) (r *Grid) {
+	r = new(Grid)
+	r.gid = gid
+	// r.minX = minX
+	// r.maxX = maxX
+	// r.minY = minY
+	// r.maxY = maxY
+	// r.minZ = minZ
+	// r.maxZ = maxZ
+	r.objectMap = make(map[int64]any)
+	return r
+}
+
+// AddObject 向格子中添加一个对象
+func (g *Grid) AddObject(objectId int64, object any) {
+	g.objectMap[objectId] = object
+}
+
+// RemoveObject 从格子中删除一个对象
+func (g *Grid) RemoveObject(objectId int64) {
+	delete(g.objectMap, objectId)
+}
+
+// GetObjectList 获取格子中所有对象
+func (g *Grid) GetObjectList() map[int64]any {
+	return g.objectMap
+}

pkg/alg/aoi_grid.go

@@ -1,53 +0,0 @@
-package alg
-
-import (
-	"hk4e/pkg/logger"
-)
-
-// Grid 地图格子
-type Grid struct {
-	gid uint32 // 格子id
-	// 格子边界坐标
-	// 目前开发阶段暂时用不到 节省点内存
-	// minX      int16
-	// maxX      int16
-	// minY      int16
-	// maxY      int16
-	// minZ      int16
-	// maxZ      int16
-	objectMap map[int64]any // k:objectId v:对象
-}
-
-// NewGrid 初始化格子
-func NewGrid(gid uint32, minX, maxX, minY, maxY, minZ, maxZ int16) (r *Grid) {
-	r = new(Grid)
-	r.gid = gid
-	// r.minX = minX
-	// r.maxX = maxX
-	// r.minY = minY
-	// r.maxY = maxY
-	// r.minZ = minZ
-	// r.maxZ = maxZ
-	r.objectMap = make(map[int64]any)
-	return r
-}
-
-// AddObject 向格子中添加一个对象
-func (g *Grid) AddObject(objectId int64, object any) {
-	g.objectMap[objectId] = object
-}
-
-// RemoveObject 从格子中删除一个对象
-func (g *Grid) RemoveObject(objectId int64) {
-	_, exist := g.objectMap[objectId]
-	if exist {
-		delete(g.objectMap, objectId)
-	} else {
-		logger.Error("remove object id but it not exist, objectId: %v", objectId)
-	}
-}
-
-// GetObjectList 获取格子中所有对象
-func (g *Grid) GetObjectList() map[int64]any {
-	return g.objectMap
-}

pkg/alg/aoi_test.go

@@ -1,19 +1,11 @@
 package alg
 
 import (
+	"log"
 	"testing"
-
-	"hk4e/common/config"
-	"hk4e/pkg/logger"
 )
 
 func TestAoiManagerGetSurrGridListByGid(t *testing.T) {
-	filePath := "./application.toml"
-	config.InitConfig(filePath)
-	logger.InitLogger("")
-	defer func() {
-		logger.CloseLogger()
-	}()
 	aoiManager := NewAoiManager()
 	aoiManager.SetAoiRange(
 		-150, 150,
@@ -25,11 +17,11 @@ func TestAoiManagerGetSurrGridListByGid(t *testing.T) {
 		// 得到当前格子周边的九宫格
 		gridList := aoiManager.GetSurrGridListByGid(k)
 		// 得到九宫格所有的id
-		logger.Debug("gid: %d gridList len: %d", k, len(gridList))
+		log.Printf("gid: %d gridList len: %d", k, len(gridList))
 		gidList := make([]uint32, 0, len(gridList))
 		for _, grid := range gridList {
 			gidList = append(gidList, grid.gid)
 		}
-		logger.Debug("Grid: gid: %d, surr grid gid list: %v", k, gidList)
+		log.Printf("Grid: gid: %d, surr grid gid list: %v", k, gidList)
 	}
 }

pkg/alg/queue_test.go

@@ -1,7 +1,7 @@
 package alg
 
 import (
-	"fmt"
+	"log"
 	"testing"
 )
 
@@ -15,7 +15,7 @@ func TestLLQueue(t *testing.T) {
 	queue.EnQueue(5)
 	for queue.Len() > 0 {
 		value := queue.DeQueue()
-		fmt.Println(value)
+		log.Println(value)
 	}
 }
 
@@ -27,7 +27,7 @@ func TestALQueue(t *testing.T) {
 	queue.EnQueue(9)
 	for queue.Len() > 0 {
 		value := queue.DeQueue()
-		fmt.Println(value)
+		log.Println(value)
 	}
 }
 
@@ -39,7 +39,7 @@ func TestRAQueue(t *testing.T) {
 	queue.EnQueue(9)
 	for queue.Len() > 0 {
 		value := queue.DeQueue()
-		fmt.Println(value)
+		log.Println(value)
 	}
 }
 

pkg/alg/shape.go

@@ -0,0 +1,255 @@
+package alg
+
+import (
+	"math"
+)
+
+// 空间形状检测
+// 默认为左手坐标系 Y轴向上 兼容Unity3D
+
+// Shape 形状
+type Shape struct {
+	region []RegionShape // 构成整个区域的组合形状集合
+}
+
+// NewShape 新建形状对象
+func NewShape() (r *Shape) {
+	r = new(Shape)
+	r.region = make([]RegionShape, 0)
+	return r
+}
+
+// RegionShape 形状抽象接口
+type RegionShape interface {
+}
+
+// RegionCubic 立方体
+type RegionCubic struct {
+	pos  *Vector3 // 几何中心
+	size *Vector3 // 三维尺寸
+}
+
+// NewCubic 新建立方体
+func (s *Shape) NewCubic(pos *Vector3, size *Vector3) {
+	if pos == nil || size == nil || size.X <= 0.0 || size.Y <= 0.0 || size.Z <= 0.0 {
+		return
+	}
+	regionCubic := &RegionCubic{
+		pos:  &Vector3{X: pos.X, Y: pos.Y, Z: pos.Z},
+		size: &Vector3{X: size.X, Y: size.Y, Z: size.Z},
+	}
+	s.region = append(s.region, regionCubic)
+}
+
+// RegionSphere 球体
+type RegionSphere struct {
+	pos    *Vector3 // 球心
+	radius float32  // 半径
+}
+
+// NewSphere 新建球体
+func (s *Shape) NewSphere(pos *Vector3, radius float32) {
+	if pos == nil || radius <= 0.0 {
+		return
+	}
+	regionSphere := &RegionSphere{
+		pos:    &Vector3{X: pos.X, Y: pos.Y, Z: pos.Z},
+		radius: radius,
+	}
+	s.region = append(s.region, regionSphere)
+}
+
+// RegionCylinder 圆柱体
+type RegionCylinder struct {
+	pos    *Vector3 // 几何中心
+	radius float32  // 半径
+	height float32  // 高度
+}
+
+// NewCylinder 新建圆柱体
+func (s *Shape) NewCylinder(pos *Vector3, radius float32, height float32) {
+	if pos == nil || radius <= 0.0 || height <= 0.0 {
+		return
+	}
+	regionCylinder := &RegionCylinder{
+		pos:    &Vector3{X: pos.X, Y: pos.Y, Z: pos.Z},
+		radius: radius,
+		height: height,
+	}
+	s.region = append(s.region, regionCylinder)
+}
+
+// RegionPolygon 空间多边形
+type RegionPolygon struct {
+	pos        *Vector3   // 几何中心
+	pointArray []*Vector2 // 多边形平面顶点数组
+	height     float32    // 高度
+}
+
+// NewPolygon 新建空间多边形
+func (s *Shape) NewPolygon(pos *Vector3, pointArray []*Vector2, height float32) {
+	if pos == nil || pointArray == nil || len(pointArray) < 3 || height <= 0.0 {
+		return
+	}
+	regionPolygon := &RegionPolygon{
+		pos:        &Vector3{X: pos.X, Y: pos.Y, Z: pos.Z},
+		pointArray: make([]*Vector2, 0),
+		height:     height,
+	}
+	for _, vector2 := range pointArray {
+		regionPolygon.pointArray = append(regionPolygon.pointArray, &Vector2{X: vector2.X, Z: vector2.Z})
+	}
+	s.region = append(s.region, regionPolygon)
+}
+
+// Clear 清除组合形状
+func (s *Shape) Clear() {
+	s.region = make([]RegionShape, 0)
+}
+
+// Contain 检测一个点是否在组合区域内
+func (s *Shape) Contain(pos *Vector3) bool {
+	for _, shape := range s.region {
+		switch shape.(type) {
+		case *RegionCubic:
+			cubic := shape.(*RegionCubic)
+			contain := regionCubicContainPos(cubic, pos)
+			if contain {
+				return true
+			}
+		case *RegionSphere:
+			sphere := shape.(*RegionSphere)
+			contain := regionSphereContainPos(sphere, pos)
+			if contain {
+				return true
+			}
+		case *RegionCylinder:
+			cylinder := shape.(*RegionCylinder)
+			contain := regionCylinderContainPos(cylinder, pos)
+			if contain {
+				return true
+			}
+		case *RegionPolygon:
+			polygon := shape.(*RegionPolygon)
+			contain := regionPolygonContainPos(polygon, pos)
+			if contain {
+				return true
+			}
+		default:
+			return false
+		}
+	}
+	return false
+}
+
+// 检测一个点是否在立方体内
+func regionCubicContainPos(cubic *RegionCubic, pos *Vector3) bool {
+	cubicMinX := cubic.pos.X - cubic.size.X
+	cubicMinY := cubic.pos.Y - cubic.size.Y
+	cubicMinZ := cubic.pos.Z - cubic.size.Z
+	cubicMaxX := cubic.pos.X + cubic.size.X
+	cubicMaxY := cubic.pos.Y + cubic.size.Y
+	cubicMaxZ := cubic.pos.Z + cubic.size.Z
+	if (pos.X > cubicMinX && pos.X < cubicMaxX) &&
+		(pos.Y > cubicMinY && pos.Y < cubicMaxY) &&
+		(pos.Z > cubicMinZ && pos.Z < cubicMaxZ) {
+		return true
+	} else {
+		return false
+	}
+}
+
+// 检测一个点是否在球体内
+func regionSphereContainPos(sphere *RegionSphere, pos *Vector3) bool {
+	distance3D := math.Sqrt(math.Pow(float64(sphere.pos.X-pos.X), 2) +
+		math.Pow(float64(sphere.pos.Y-pos.Y), 2) +
+		math.Pow(float64(sphere.pos.Z-pos.Z), 2))
+	if float32(distance3D) < sphere.radius {
+		return true
+	} else {
+		return false
+	}
+}
+
+// 检测一个点是否在圆柱体内
+func regionCylinderContainPos(cylinder *RegionCylinder, pos *Vector3) bool {
+	distance2D := math.Sqrt(math.Pow(float64(cylinder.pos.X-pos.X), 2) +
+		math.Pow(float64(cylinder.pos.Z-pos.Z), 2))
+	if float32(distance2D) >= cylinder.radius {
+		return false
+	}
+	cylinderMinY := cylinder.pos.Y - (cylinder.height / 2.0)
+	cylinderMaxY := cylinder.pos.Y + (cylinder.height / 2.0)
+	if pos.Y > cylinderMinY && pos.Y < cylinderMaxY {
+		return true
+	} else {
+		return false
+	}
+}
+
+// 检测一个点是否在空间多边形内
+func regionPolygonContainPos(polygon *RegionPolygon, pos *Vector3) bool {
+	contain := region2DPolygonContainPos(polygon.pointArray, &Vector2{X: pos.X, Z: pos.Z})
+	if !contain {
+		return false
+	}
+	polygonMinY := polygon.pos.Y - (polygon.height / 2.0)
+	polygonMaxY := polygon.pos.Y + (polygon.height / 2.0)
+	if pos.Y > polygonMinY && pos.Y < polygonMaxY {
+		return true
+	} else {
+		return false
+	}
+}
+
+// 检测一个点是否在平面多边形内
+func region2DPolygonContainPos(pointArray []*Vector2, pos *Vector2) bool {
+	convexPolygonList := make([][]*Vector2, 0)
+	// TODO 凹多边形分割为多个凸多边形
+	convexPolygonList = append(convexPolygonList, pointArray)
+	for _, convexPolygon := range convexPolygonList {
+		contain := region2DConvexPolygonContainPos(convexPolygon, pos)
+		if contain {
+			return true
+		}
+	}
+	return false
+}
+
+// 检测一个点是否在平面凸多边形内
+func region2DConvexPolygonContainPos(pointArray []*Vector2, pos *Vector2) bool {
+	// 凸多边形分割为多个三角形
+	for index := range pointArray {
+		if index < 2 {
+			continue
+		}
+		contain := inTriangle(pointArray[index], pointArray[index-1], pointArray[0], pos)
+		if contain {
+			return true
+		}
+	}
+	return false
+}
+
+func inTriangle(a *Vector2, b *Vector2, c *Vector2, p *Vector2) bool {
+	// 三角形顶点逆时针排序
+	ab := Vector3Sub(&Vector3{X: b.X, Y: 0.0, Z: b.Z}, &Vector3{X: a.X, Y: 0.0, Z: a.Z})
+	ac := Vector3Sub(&Vector3{X: c.X, Y: 0.0, Z: c.Z}, &Vector3{X: a.X, Y: 0.0, Z: a.Z})
+	vp := Vector3CrossProd(ab, ac)
+	if vp.Y > 0.0 {
+		tmp := &Vector2{X: b.X, Z: b.Z}
+		b = c
+		c = tmp
+	}
+	return toLeft(a, b, c, p) && toLeft(b, c, a, p) && toLeft(c, a, b, p)
+}
+
+func toLeft(a *Vector2, b *Vector2, c *Vector2, p *Vector2) bool {
+	ab := Vector3Sub(&Vector3{X: b.X, Y: 0.0, Z: b.Z}, &Vector3{X: a.X, Y: 0.0, Z: a.Z})
+	ac := Vector3Sub(&Vector3{X: c.X, Y: 0.0, Z: c.Z}, &Vector3{X: a.X, Y: 0.0, Z: a.Z})
+	ap := Vector3Sub(&Vector3{X: p.X, Y: 0.0, Z: p.Z}, &Vector3{X: a.X, Y: 0.0, Z: a.Z})
+	v1 := Vector3CrossProd(ab, ac)
+	v2 := Vector3CrossProd(ab, ap)
+	dp := Vector3DotProd(v1, v2)
+	return dp >= 0.0
+}

pkg/alg/shape_test.go

@@ -0,0 +1,41 @@
+package alg
+
+import (
+	"log"
+	"testing"
+	"time"
+)
+
+func TestShape(t *testing.T) {
+	shape := NewShape()
+	shape.NewCubic(&Vector3{X: 5.0, Y: 0.0, Z: 5.0}, &Vector3{X: 5.0, Y: 10.0, Z: 5.0})
+	shape.NewSphere(&Vector3{X: -10.0, Y: -10.0, Z: -10.0}, 10.0)
+	shape.NewCylinder(&Vector3{X: 0.0, Y: 0.0, Z: 0.0}, 3.0, 5.0)
+	shape.NewPolygon(&Vector3{X: 0.0, Y: 0.0, Z: 0.0}, []*Vector2{
+		{X: 10.0, Z: 10.0},
+		{X: 20.0, Z: 0.0},
+		{X: 10.0, Z: -10.0},
+		{X: -10.0, Z: -10.0},
+		{X: -20.0, Z: 0.0},
+		{X: -10.0, Z: 10.0},
+	}, 10.0)
+	contain := shape.Contain(&Vector3{X: 5.0, Y: 0.0, Z: 5.0})
+	log.Printf("contain: %v\n", contain)
+	startTime := time.Now().UnixNano()
+	for i := 0; i < 1000*1000*10; i++ {
+		shape.Contain(&Vector3{X: 999.9, Y: 888.8, Z: 777.7})
+	}
+	endTime := time.Now().UnixNano()
+	log.Printf("avg cost time: %v ns\n", (endTime-startTime)/(1000*1000*10))
+	shape.Clear()
+	shape.NewPolygon(&Vector3{X: 0.0, Y: 0.0, Z: 0.0}, []*Vector2{
+		{X: 1.0, Z: 1.0},
+		{X: 2.0, Z: 0.0},
+		{X: 1.0, Z: -1.0},
+		{X: -1.0, Z: -1.0},
+		{X: -2.0, Z: 0.0},
+		{X: -1.0, Z: 1.0},
+	}, 10.0)
+	polygonContain := shape.Contain(&Vector3{X: 0.1, Y: 0.0, Z: 0.1})
+	log.Printf("polygon contain: %v\n", polygonContain)
+}

pkg/alg/snowflake_test.go

@@ -1,7 +1,7 @@
 package alg
 
 import (
-	"fmt"
+	"log"
 	"sync"
 	"testing"
 )
@@ -32,13 +32,13 @@ func idDupCheck[T UniqueID](genIdFunc func() T) {
 		for _, id := range *idListPtr {
 			value, exist := dupCheck[id]
 			if exist && value == true {
-				fmt.Printf("find dup id, gid: %v, id: %v\n", gid, id)
+				log.Printf("find dup id, gid: %v, id: %v\n", gid, id)
 			} else {
 				dupCheck[id] = true
 			}
 		}
 	}
-	fmt.Printf("check finish\n")
+	log.Printf("check finish\n")
 }
 
 func TestSnowflakeGenId(t *testing.T) {

pkg/alg/vector.go

@@ -0,0 +1,67 @@
+package alg
+
+// Vector2 二维向量
+type Vector2 struct {
+	X float32
+	Z float32
+}
+
+// Vector3 三维向量
+type Vector3 struct {
+	X float32
+	Y float32
+	Z float32
+}
+
+// Vector2Add 二维向量加
+func Vector2Add(v1 *Vector2, v2 *Vector2) *Vector2 {
+	v3 := new(Vector2)
+	v3.X = v1.X + v2.X
+	v3.Z = v1.Z + v2.Z
+	return v3
+}
+
+// Vector2Sub 二维向量减
+func Vector2Sub(v1 *Vector2, v2 *Vector2) *Vector2 {
+	v3 := new(Vector2)
+	v3.X = v1.X - v2.X
+	v3.Z = v1.Z - v2.Z
+	return v3
+}
+
+// Vector2DotProd 二维向量点乘
+func Vector2DotProd(v1 *Vector2, v2 *Vector2) float32 {
+	return v1.X*v2.X + v1.Z*v2.Z
+}
+
+// Vector3Add 三维向量加
+func Vector3Add(v1 *Vector3, v2 *Vector3) *Vector3 {
+	v3 := new(Vector3)
+	v3.X = v1.X + v2.X
+	v3.Y = v1.Y + v2.Y
+	v3.Z = v1.Z + v2.Z
+	return v3
+}
+
+// Vector3Sub 三维向量减
+func Vector3Sub(v1 *Vector3, v2 *Vector3) *Vector3 {
+	v3 := new(Vector3)
+	v3.X = v1.X - v2.X
+	v3.Y = v1.Y - v2.Y
+	v3.Z = v1.Z - v2.Z
+	return v3
+}
+
+// Vector3DotProd 三维向量点乘
+func Vector3DotProd(v1 *Vector3, v2 *Vector3) float32 {
+	return v1.X*v2.X + v1.Y*v2.Y + v1.Z*v2.Z
+}
+
+// Vector3CrossProd 三维向量叉乘
+func Vector3CrossProd(v1 *Vector3, v2 *Vector3) *Vector3 {
+	v3 := new(Vector3)
+	v3.X = v1.Y*v2.Z - v2.Y*v1.Z
+	v3.Y = v2.X*v1.Z - v2.Z*v1.X
+	v3.Z = v1.X*v2.Y - v2.X*v1.Y
+	return v3
+}