269 lines
7.4 KiB
Go
269 lines
7.4 KiB
Go
package device_sys
|
||
|
||
import (
|
||
"fmt"
|
||
"log/slog"
|
||
"strings"
|
||
|
||
"joylink.club/ecs"
|
||
"joylink.club/ecs/filter"
|
||
"joylink.club/rtsssimulation/component"
|
||
"joylink.club/rtsssimulation/entity"
|
||
"joylink.club/rtsssimulation/repository"
|
||
"joylink.club/rtsssimulation/repository/model/proto"
|
||
)
|
||
|
||
// SectionDetectSystem 区段检测系统
|
||
type SectionDetectSystem struct {
|
||
trainQuery *ecs.Query
|
||
axleSectionQuery *ecs.Query
|
||
}
|
||
|
||
func NewSectionDetectSystem() *SectionDetectSystem {
|
||
return &SectionDetectSystem{trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType)),
|
||
axleSectionQuery: ecs.NewQuery(filter.Contains(component.UidType, component.AxlePhysicalSectionType))}
|
||
}
|
||
func (s *SectionDetectSystem) Update(w ecs.World) {
|
||
//key-sectionId,统计区段上有车的情况
|
||
sectionTrainMap := make(map[string]*trainCount)
|
||
//所有列车
|
||
s.trainQuery.Each(w, func(entry *ecs.Entry) {
|
||
tp := component.TrainPositionInfoType.Get(entry)
|
||
//fmt.Println("============>>>>>>列车位置信息：", tp.ToString())
|
||
trainSectionIds := s.doSearchTrainOccupiedSections(w, tp)
|
||
//fmt.Println("============>>>>>>列车所在物理区段：", trainSectionIds)
|
||
for _, sectionId := range trainSectionIds { //车所在区段
|
||
tc, find := sectionTrainMap[sectionId]
|
||
if !find {
|
||
tc = newTrainCount()
|
||
sectionTrainMap[sectionId] = tc
|
||
}
|
||
tc.add()
|
||
}
|
||
})
|
||
//计轴区段
|
||
s.axleSectionQuery.Each(w, func(entry *ecs.Entry) {
|
||
axleSectionId := component.UidType.Get(entry).Id
|
||
axleSection := component.AxlePhysicalSectionType.Get(entry)
|
||
tc, find := sectionTrainMap[axleSectionId]
|
||
if find {
|
||
axleSection.UpdateCount(int(tc.count))
|
||
} else {
|
||
axleSection.UpdateCount(0)
|
||
}
|
||
})
|
||
}
|
||
|
||
type trainCount struct {
|
||
count int8
|
||
}
|
||
|
||
func newTrainCount() *trainCount {
|
||
return &trainCount{count: 0}
|
||
}
|
||
func (c *trainCount) add() {
|
||
c.count++
|
||
}
|
||
func (s *SectionDetectSystem) doSearchTrainOccupiedSections(w ecs.World, tp *component.TrainPositionInfo) []string {
|
||
wd := entity.GetWorldData(w)
|
||
curLink := wd.Repo.FindLink(tp.HeadLink)
|
||
stp := &stpContext{w: w, trainLen: tp.Len, curLink: curLink, curOffset: tp.HeadLinkOffset, searchDirection: !tp.Up, acLen: 0}
|
||
for stp.needAccumulate() {
|
||
stp.accumulateLen()
|
||
if stp.needAccumulate() {
|
||
stp.nextLink()
|
||
} else {
|
||
break
|
||
}
|
||
}
|
||
//
|
||
return stp.trainSections()
|
||
}
|
||
|
||
/////////////////////////////////////////////////////////////////////
|
||
|
||
type stpContext struct {
|
||
w ecs.World
|
||
trainLen int64 //列车长度
|
||
curLink *repository.Link //当前迭代的link
|
||
curOffset int64 //当前迭代的link,长度累加起点
|
||
searchDirection bool //在curLink上长度累加的方向，true:a->b,false:b->a,a->b偏移量变大
|
||
acLen int64 //当前累加长度
|
||
acRanges []*stpLinkRange //轨道range列表
|
||
}
|
||
|
||
func (s *stpContext) printStpLinkRanges() {
|
||
var sb strings.Builder
|
||
for _, r := range s.acRanges {
|
||
sb.WriteString(fmt.Sprintf("[linkId=%s start=%d end=%d linkLen=%d]->", r.link.Id(), r.start, r.end, r.link.Length()))
|
||
}
|
||
fmt.Println("===========>>>>列车所在link range 链：", sb.String())
|
||
}
|
||
|
||
// 获取列车所占的物理区段
|
||
func (s *stpContext) trainSections() []string {
|
||
secs := make(map[string]string)
|
||
for _, acR := range s.acRanges {
|
||
ids := acR.sectionIds()
|
||
for _, id := range ids {
|
||
secs[id] = id
|
||
}
|
||
}
|
||
//
|
||
//s.printStpLinkRanges()
|
||
//
|
||
var sectionIds []string
|
||
for _, id := range secs {
|
||
sectionIds = append(sectionIds, id)
|
||
}
|
||
return sectionIds
|
||
}
|
||
func (s *stpContext) needAccumulate() bool {
|
||
return s.curLink != nil && s.acLen < s.trainLen
|
||
}
|
||
func (s *stpContext) nextLink() {
|
||
var nextLinkPort *repository.LinkPort
|
||
if s.searchDirection {
|
||
bDc := s.curLink.BRelation()
|
||
bDcDw := s.turnoutPosition(bDc.Turnout().Id())
|
||
switch bDc.Port() {
|
||
case proto.Port_A: //link-b连接turnout-A
|
||
if bDcDw {
|
||
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_B)
|
||
} else {
|
||
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_C)
|
||
}
|
||
case proto.Port_B: //link-b连接turnout-B
|
||
fallthrough
|
||
case proto.Port_C: //link-b连接turnout-C
|
||
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_A)
|
||
}
|
||
} else {
|
||
aDc := s.curLink.ARelation()
|
||
aDcDw := s.turnoutPosition(aDc.Turnout().Id())
|
||
switch aDc.Port() {
|
||
case proto.Port_A: //link-a连接turnout-A
|
||
if aDcDw {
|
||
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_B)
|
||
} else {
|
||
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_C)
|
||
}
|
||
case proto.Port_B: //link-a连接turnout-B
|
||
fallthrough
|
||
case proto.Port_C: //link-a连接turnout-C
|
||
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_A)
|
||
}
|
||
}
|
||
//
|
||
if nextLinkPort != nil {
|
||
s.curLink = nextLinkPort.Device().(*repository.Link)
|
||
fromA := nextLinkPort.IsPortA()
|
||
s.searchDirection = fromA
|
||
if fromA {
|
||
s.curOffset = 0
|
||
} else {
|
||
s.curOffset = s.curLink.Length()
|
||
}
|
||
} else {
|
||
slog.Warn("区段检测时，获取列车下的link失败")
|
||
}
|
||
}
|
||
|
||
// 获取道岔实际位置
|
||
func (s *stpContext) turnoutPosition(id string) bool {
|
||
wd := entity.GetWorldData(s.w)
|
||
entry, ok := wd.EntityMap[id]
|
||
if ok {
|
||
return component.TurnoutPositionType.Get(entry).Dw
|
||
} else {
|
||
panic(fmt.Sprintf("道岔[%s]的实体不存在", id))
|
||
}
|
||
}
|
||
|
||
// 累加
|
||
func (s *stpContext) accumulateLen() {
|
||
if s.searchDirection {
|
||
dl := s.curLink.Length() - s.curOffset
|
||
if s.acLen+dl < s.trainLen {
|
||
s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset, end: s.curLink.Length()})
|
||
s.acLen += dl
|
||
} else {
|
||
tl := s.trainLen - s.acLen
|
||
s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset, end: s.curOffset + tl})
|
||
s.acLen += tl
|
||
}
|
||
} else {
|
||
dl := s.curOffset
|
||
if s.acLen+dl < s.trainLen {
|
||
s.addLinkRange(&stpLinkRange{link: s.curLink, start: 0, end: s.curOffset})
|
||
s.acLen += dl
|
||
} else {
|
||
tl := s.trainLen - s.acLen
|
||
s.addLinkRange(&stpLinkRange{link: s.curLink, start: s.curOffset - tl, end: s.curOffset})
|
||
s.acLen += tl
|
||
}
|
||
}
|
||
}
|
||
func (s *stpContext) addLinkRange(r *stpLinkRange) {
|
||
s.acRanges = append(s.acRanges, r)
|
||
}
|
||
|
||
///////////////////////////////////////////////////////////////
|
||
|
||
type stpLinkRange struct {
|
||
link *repository.Link
|
||
start int64
|
||
end int64
|
||
}
|
||
|
||
// 获取link range 上的物理区段的id
|
||
func (s *stpLinkRange) sectionIds() []string {
|
||
var ids []string
|
||
ps := s.link.GetAllPhysicalSection()
|
||
for _, p := range ps {
|
||
for _, pr := range p.LinkRanges() {
|
||
if isRangeOverlap(s.link.Id(), s.start, s.end, pr.Link().Id(), pr.Start(), pr.End()) {
|
||
ids = append(ids, p.Id())
|
||
}
|
||
}
|
||
}
|
||
return ids
|
||
}
|
||
func isRangeOverlap(r1LinkId string, r1Start int64, r1End int64, r2LinkId string, r2Start int64, r2End int64) bool {
|
||
if r1LinkId != r2LinkId {
|
||
return false
|
||
}
|
||
r1Start, r1End = formatRange(r1Start, r1End)
|
||
r2Start, r2End = formatRange(r2Start, r2End)
|
||
if r2Start > r1Start && r2Start < r1End {
|
||
return true
|
||
}
|
||
if r2End > r1Start && r2End < r1End {
|
||
return true
|
||
}
|
||
if r1Start > r2Start && r1Start < r2End {
|
||
return true
|
||
}
|
||
if r1End > r2Start && r1End < r2End {
|
||
return true
|
||
}
|
||
//
|
||
return false
|
||
}
|
||
func formatRange(start int64, end int64) (int64, int64) {
|
||
if start < end {
|
||
return start, end
|
||
} else {
|
||
return end, start
|
||
}
|
||
}
|
||
func printPhSectionRange(w ecs.World, id string) string {
|
||
wd := entity.GetWorldData(w)
|
||
var sb strings.Builder
|
||
sb.WriteString(fmt.Sprintf("物理区段[%s]range列表：", id))
|
||
for _, r := range wd.Repo.FindPhysicalSection(id).LinkRanges() {
|
||
sb.WriteString(fmt.Sprintf("[linkId=%s start=%d end=%d],", r.Link().Id(), r.Start(), r.End()))
|
||
}
|
||
return sb.String()
|
||
}
|