cicv_data_closed_loop/trigger/pjisuv/map_polygon/OutOfLane/main/OutOfLane.go

package main

import (
	"cicv-data-closedloop/pjisuv_msgs"
	"cicv-data-closedloop/pjisuv_param"
	"fmt"
	"math"
)

func Topic() string {
	return "/map_polygon"
}

func Label() string {
	return "OutOfLane"
}

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 *pjisuv_param.PjisuvParam) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	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) {
			return "OutOfLane"
		}
	}
	return ""
}