From cf648670bfc6426f31e414b37e959fb6458779d6 Mon Sep 17 00:00:00 2001 From: xzb <223@qq.com> Date: Fri, 10 Nov 2023 16:03:33 +0800 Subject: [PATCH] =?UTF-8?q?=E8=AE=A1=E8=BD=B4rssp=20=E9=9B=86=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- component/train.go | 21 +-- fi/train.go | 33 ++--- repository/link.go | 3 + repository/physical_section.go | 3 + repository/turnout.go | 12 +- sys/device_sys/section_detection.to.go | 190 ++++++++++++++++++++++++- 6 files changed, 233 insertions(+), 29 deletions(-) diff --git a/component/train.go b/component/train.go index 0c70316..43173ef 100644 --- a/component/train.go +++ b/component/train.go @@ -4,16 +4,19 @@ import "joylink.club/ecs" // TrainPositionInfo 列车当前位置信息 type TrainPositionInfo struct { - //列车所占的物理区段 - SectionIds []string - //列车长度 mm - Len uint32 - //列车所在轨道link - Link uint8 - //列车所在link偏移量（mm） - LinkOffset uint32 - //列车当前运行方向（偏移量增大/减小方向） + //列车头当前运行方向（true偏移量增大/false减小方向） + //link 由a->b偏移量增大 Up bool + //列车长度 mm + Len int64 + //列车所在轨道link + HeadLink string + //列车所在link偏移量（mm） + HeadLinkOffset int64 + //列车所在轨道link + TailLink string + //列车所在link偏移量（mm） + TailLinkOffset int64 } var ( diff --git a/fi/train.go b/fi/train.go index f30f56c..93acd69 100644 --- a/fi/train.go +++ b/fi/train.go @@ -2,7 +2,6 @@ package fi import ( "fmt" - "joylink.club/ecs" "joylink.club/rtsssimulation/component" "joylink.club/rtsssimulation/entity" @@ -35,26 +34,24 @@ func RemoveTrainFromWorld(w ecs.World, trainId string) error { return result.Err } -// UpdateTrainFromDynamics 更新列车所在的物理区段 -func UpdateTrainFromDynamics(w ecs.World, trainId string, sectionIds []string) error { - result := <-ecs.Request[ecs.EmptyType](w, func() ecs.Result[ecs.EmptyType] { - wd := entity.GetWorldData(w) - te, find := wd.EntityMap[trainId] - if find { - tp := component.TrainPositionInfoType.Get(te) - tp.SectionIds = sectionIds - return ecs.NewOkEmptyResult() - } else { - return ecs.NewErrResult(fmt.Errorf("列车[%s]实体不存在", trainId)) - } - }) - return result.Err -} - // UpdateTrainPositionFromDynamics 更新列车所在的物理区段 func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error { result := <-ecs.Request[ecs.EmptyType](w, func() ecs.Result[ecs.EmptyType] { - return ecs.NewOkEmptyResult() + wd := entity.GetWorldData(w) + te, find := wd.EntityMap[tpi.TrainId] + if find { + train := component.TrainPositionInfoType.Get(te) + train.Up = tpi.Up + train.Len = int64(tpi.Len) + train.HeadLink = tpi.HeadLink + train.HeadLinkOffset = int64(tpi.TailLinkOffset) + train.TailLink = tpi.TailLink + train.TailLinkOffset = int64(tpi.HeadLinkOffset) + return ecs.NewOkEmptyResult() + } else { + return ecs.NewErrResult(fmt.Errorf("列车[%s]实体不存在", tpi.TrainId)) + } + }) return result.Err } diff --git a/repository/link.go b/repository/link.go index 4720cc1..fd186a9 100644 --- a/repository/link.go +++ b/repository/link.go @@ -140,6 +140,9 @@ func (l *LinkPort) Port() proto.Port { func (l *LinkPort) Device() PortedDevice { return l.link } +func (l *LinkPort) IsPortA() bool { + return l.port == proto.Port_A +} type LinkRange struct { link *Link diff --git a/repository/physical_section.go b/repository/physical_section.go index c678309..83ea850 100644 --- a/repository/physical_section.go +++ b/repository/physical_section.go @@ -176,6 +176,9 @@ func (s *PhysicalSection) findBoundaryKmByPort(port proto.Port) *proto.Kilometer } return nil } +func (s *PhysicalSection) LinkRanges() []*LinkRange { + return s.linkRanges +} type PhysicalSectionPort struct { section *PhysicalSection diff --git a/repository/turnout.go b/repository/turnout.go index b72d987..dc2416c 100644 --- a/repository/turnout.go +++ b/repository/turnout.go @@ -244,7 +244,17 @@ func (t *Turnout) FindLinkPositionByPort(port proto.Port) *LinkPosition { } return nil } - +func (t *Turnout) FindLinkByPort(port proto.Port) *LinkPort { + switch port { + case proto.Port_A: + return t.aLinkPort + case proto.Port_B: + return t.bLinkPort + case proto.Port_C: + return t.cLinkPort + } + return nil +} func (t *Turnout) SwitchMachineType() proto.Turnout_SwitchMachineType { return t.switchMachineType } diff --git a/sys/device_sys/section_detection.to.go b/sys/device_sys/section_detection.to.go index b7fd8f1..6705539 100644 --- a/sys/device_sys/section_detection.to.go +++ b/sys/device_sys/section_detection.to.go @@ -1,10 +1,15 @@ package device_sys import ( + "fmt" "github.com/yohamta/donburi" "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" + "log/slog" ) // SectionDetectSystem 区段检测系统 @@ -23,7 +28,8 @@ func (s *SectionDetectSystem) Update(w ecs.World) { //所有列车 s.trainQuery.Each(w, func(entry *donburi.Entry) { tp := component.TrainPositionInfoType.Get(entry) - for _, sectionId := range tp.SectionIds { //车所在区段 + trainSectionIds := s.doSearchTrainOccupiedSections(w, tp) + for _, sectionId := range trainSectionIds { //车所在区段 tc, find := sectionTrainMap[sectionId] if !find { tc = newTrainCount() @@ -55,3 +61,185 @@ func newTrainCount() *trainCount { 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} + c := 0 + for stp.needAccumulate() { + c++ + if c > 50 { + slog.Warn("======>>>>搜索列车所占物理区段，迭代次数过多!!!!") + break + } + 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) trainSections() []string { + secs := make(map[string]string) + for _, acR := range s.acRanges { + ids := acR.sectionIds() + for _, id := range ids { + secs[id] = id + } + } + 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.PhysicalSections() + for _, p := range ps { + for _, pr := range p.LinkRanges() { + if isRangeOverlap(s.start, s.end, pr.Start(), pr.End()) { + ids = append(ids, p.Id()) + } + } + } + return ids +} +func isRangeOverlap(r1Start int64, r1End int64, r2Start int64, r2End int64) bool { + 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 && r1End == r2End { + return true + } + return false +} +func formatRange(start int64, end int64) (int64, int64) { + if start < end { + return start, end + } else { + return end, start + } +}