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

package main

//触发器名称：冲出车道

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

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

func Label() string {
	return "OutOfLane"
}

func Rule(shareVars *sync.Map, data *pjisuv_msgs.PolygonStamped) string {
	defer func() {
		if r := recover(); r != nil {
			fmt.Println("Recovered from panic:", r)
		}
	}()
	positionXOfCicvLocation, ok1 := shareVars.Load("PositionXOfCicvLocation")
	positionYOfCicvLocation, ok2 := shareVars.Load("PositionYOfCicvLocation")
	yawOfCicvLocation, ok3 := shareVars.Load("YawOfCicvLocation")
	automodeOfPjVehicleFdbPub, ok4 := shareVars.Load("AutomodeOfPjVehicleFdbPub")
	if ok1 && ok2 && ok3 && ok4 {
		Points := data.Polygon.Points
		D1, D2, D3 := 1.0, 0.4, 0.4 //这是实车缩小后的尺寸
		corners := getVehicleCorners(positionXOfCicvLocation.(float64), positionYOfCicvLocation.(float64), D1, D2, D3, yawOfCicvLocation.(float64))
		for i := 0; i < len(Points)-1; i++ {
			A := Points[i]
			B := Points[i+1]
			if polygonLineIntersect(corners, A, B) && automodeOfPjVehicleFdbPub.(int16) == 1 {
				return Label()
			}
		}
	}
	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
}