rts-sim-module/sys/device_sys/balise_detection.go

package device_sys

import (
	"joylink.club/ecs"
	"joylink.club/ecs/filter"
	"joylink.club/rtsssimulation/component"
	"joylink.club/rtsssimulation/entity"
	"joylink.club/rtsssimulation/repository"
)

// BaliseDetectSystem 列车应答器天线探测轨旁应答器
// 一条应答器报文长830bits;单个应答器对应多条报文；
// 固定应答器对应1条报文；默认报文对应1条；
type BaliseDetectSystem struct {
	trainQuery *ecs.Query
}

func NewBaliseDetectSystem() *BaliseDetectSystem {
	return &BaliseDetectSystem{
		trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)),
	}
}
func (s *BaliseDetectSystem) Update(w ecs.World) {
	wd := entity.GetWorldData(w)
	balises := wd.Repo.ResponderList()
	//所有列车
	//列车速度80KM/H时，222mm/10ms
	s.trainQuery.Each(w, func(entry *ecs.Entry) {
		tp := component.TrainPositionInfoType.Get(entry)
		detectedBalise := s.detect(wd, tp, balises)
		if detectedBalise != nil { //列车应答器天线扫描到应答器,获取应答器报文发送给BTM

		}
	})
}
func (s *BaliseDetectSystem) detect(wd *component.WorldData, tp *component.TrainPositionInfo, balises []*repository.Transponder) *repository.Transponder {
	scanRange := s.calculateScanRange(wd, tp)
	for _, balise := range balises {
		if scanRange.contains(balise.LinkPosition()) {
			return balise
		}
	}
	return nil
}

const scanWidth = int64(1000) //1000mm
// 计算车载应答器天线扫描范围，如果应答器在此范围内则天线可以接收到应答器报文
func (s *BaliseDetectSystem) calculateScanRange(wd *component.WorldData, tp *component.TrainPositionInfo) *scanRange {
	headLink := wd.Repo.FindLink(tp.HeadLink)
	if tp.Up { //列车运行方向a->b
		if tp.HeadLinkOffset >= scanWidth {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset-scanWidth, tp.HeadLinkOffset)
		} else {
			return newScanRange(headLink.Id(), 0, tp.HeadLinkOffset)
		}
	} else { //列车运行方向b->a
		if headLink.Length()-tp.HeadLinkOffset >= scanWidth {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset, tp.HeadLinkOffset+scanWidth)
		} else {
			return newScanRange(headLink.Id(), tp.HeadLinkOffset, headLink.Length())
		}
	}
}

type scanRange struct {
	linkId      string
	startOffset int64
	endOffset   int64
}

func newScanRange(linkId string, start int64, end int64) *scanRange {
	return &scanRange{linkId: linkId, startOffset: start, endOffset: end}
}
func (s *scanRange) format() {
	if s.startOffset > s.endOffset {
		s.startOffset, s.endOffset = s.endOffset, s.startOffset
	}
}

// true-应答器balisePosition在该scanRange内
func (s *scanRange) contains(balisePosition *repository.LinkPosition) bool {
	if s.linkId != balisePosition.Link().Id() {
		return false
	}
	s.format()
	return balisePosition.Offset() >= s.startOffset && balisePosition.Offset() <= s.endOffset
}