commit

trigger/pjisuv/map_polygon/outofroad/main/outofroad.go

@@ -3,6 +3,8 @@ package main
 import (
 	"cicv-data-closedloop/common/entity"
 	"cicv-data-closedloop/pjisuv_msgs"
+	"fmt"
+	"math"
 )
 
 func Topic() string {
@@ -13,10 +15,62 @@ func Label() string {
 	return "outofroad"
 }
 
-func Rule(data *pjisuv_msgs.VehicleFdb, param entity.PjisuvParam) string {
-	if data.Automode == 0 && param.AutomodeOfPjVehicleFdbPub == 1 {
-		return "outofroad"
-	} else {
-		return ""
+type Point struct {
+	X, Y float64
+}
+
+func getVehicleCorners(x0, y0, D1, D2, D3, h0 float64) []pjisuv_msgs.Point64 {
+	h0Rad := h0 * math.Pi / 180 // 转换角度到弧度
+	cosH0, sinH0 := math.Cos(h0Rad), math.Sin(h0Rad)
+
+	// 车辆四个角点相对于后轴中心点的坐标（局部坐标系）
+	cornersLocal := []Point{
+		{D1, D3},   // 前左角
+		{D1, -D3},  // 前右角
+		{-D2, D3},  // 后左角
+		{-D2, -D3}, // 后右角
+	}
+
+	// 旋转矩阵并计算全局坐标
+	rotationMatrix := [2][2]float64{{cosH0, -sinH0}, {sinH0, cosH0}}
+	cornersGlobal := make([]pjisuv_msgs.Point64, len(cornersLocal))
+	for i, corner := range cornersLocal {
+		cornersGlobal[i].X = corner.X*rotationMatrix[0][0] + corner.Y*rotationMatrix[1][0] + x0
+		cornersGlobal[i].Y = corner.X*rotationMatrix[0][1] + corner.Y*rotationMatrix[1][1] + y0
+	}
+	return cornersGlobal
+}
+
+func ccw(A, B, C pjisuv_msgs.Point64) bool {
+	return (C.Y-A.Y)*(B.X-A.X) > (C.X-A.X)*(B.Y-A.Y)
+}
+
+func lineIntersect(A, B, C, D pjisuv_msgs.Point64) bool {
+	return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D)
+}
+
+func polygonLineIntersect(polygon []pjisuv_msgs.Point64, A, B pjisuv_msgs.Point64) bool {
+	for i := 0; i < len(polygon)-1; i++ {
+		C := polygon[i]
+		D := polygon[(i+1)%len(polygon)]
+		if lineIntersect(A, B, C, D) {
+			return true
+		}
+	}
+	return false
+}
+
+func Rule(data *pjisuv_msgs.PolygonStamped, param entity.PjisuvParam) string {
+	Points := data.Polygon.Points
+	D1, D2, D3 := 3.813, 0.952, 1.086
+	corners := getVehicleCorners(param.PositionXOfCicvLocation, param.PositionYOfCicvLocation, D1, D2, D3, param.YawOfCicvLocation)
+
+	for i := 0; i < len(Points)-1; i++ {
+		A := Points[i]
+		B := Points[i+1]
+		if polygonLineIntersect(corners, A, B) {
+			fmt.Println("outofroad")
+			return
+		}
 	}
 }