001package jmri.jmrit.vsdecoder; 002 003import java.awt.geom.*; 004import java.util.ArrayList; 005import java.util.List; 006import jmri.jmrit.display.layoutEditor.*; 007import jmri.util.MathUtil; 008 009import javax.annotation.*; 010 011/** 012 * Navigation through a LayoutEditor panel to set the sound position. 013 * 014 * Almost all code from George Warner's LENavigator. 015 * ------------------------------------------------ 016 * Added direction change feature with new methods 017 * setReturnTrack(T), setReturnLastTrack(T) and 018 * a Block check. 019 * 020 * Concept for direction change, e.g.: 021 * EndBumper ---- TrackSegment ------ Anchor 022 * lastTrack returnTrack returnLastTrack 023 * 024 * <hr> 025 * This file is part of JMRI. 026 * <p> 027 * JMRI is free software; you can redistribute it and/or modify it under 028 * the terms of version 2 of the GNU General Public License as published 029 * by the Free Software Foundation. See the "COPYING" file for a copy 030 * of this license. 031 * <p> 032 * JMRI is distributed in the hope that it will be useful, but WITHOUT 033 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 034 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 035 * for more details. 036 * 037 * @author Klaus Killinger Copyright (C) 2022, 2023, 2026 038 */ 039public class VSDNavigation { 040 041 private VSDecoder d; 042 043 private boolean use_blocks = VSDecoderManager.instance().getVSDecoderPreferences().getUseBlocksSetting(); 044 045 private int lastTurntablePosition = -1; 046 047 // constructor 048 VSDNavigation(VSDecoder vsd) { 049 d = vsd; 050 } 051 052 // layout track specific methods 053 boolean navigatePositionalPoint() { 054 boolean result = true; // always go to next track 055 PositionablePoint pp = (PositionablePoint) d.getLayoutTrack(); 056 PositionablePoint.PointType type = pp.getType(); 057 switch (type) { 058 case ANCHOR: { 059 if (pp.getConnect1().equals(d.getLastTrack())) { 060 d.setLayoutTrack(pp.getConnect2()); 061 d.setReturnTrack(d.getLayoutTrack()); 062 } else if (pp.getConnect2().equals(d.getLastTrack())) { 063 d.setLayoutTrack(pp.getConnect1()); 064 d.setReturnTrack(d.getLayoutTrack()); 065 } else { // OOPS! we're lost! 066 result = false; 067 break; 068 } 069 d.setLastTrack(pp); 070 break; 071 } 072 default: 073 case END_BUMPER: { 074 d.setReturnTrack(pp.getConnect1()); 075 d.distanceOnTrack = d.getReturnDistance(); 076 d.setDistance(0); 077 result = false; 078 break; 079 } 080 case EDGE_CONNECTOR: { 081 TrackSegment ts2 = null; 082 if (pp.getLinkedPoint() != null) { 083 ts2 = pp.getLinkedPoint().getConnect1(); 084 d.setModels(pp.getLinkedEditor()); // change the panel 085 d.setLayoutTrack(ts2); 086 d.setReturnTrack(d.getLayoutTrack()); 087 if (pp.getLinkedPoint().equals(ts2.getConnect1())) { 088 d.setLastTrack(ts2.getConnect1()); 089 } else if (pp.getLinkedPoint().equals(ts2.getConnect2())) { 090 d.setLastTrack(ts2.getConnect2()); 091 } else { 092 log.warn(" EdgeConnector lost"); 093 } 094 } else { 095 log.warn(" EdgeConnector is not linked"); 096 d.setReturnTrack(d.getLastTrack()); 097 d.distanceOnTrack = d.getReturnDistance(); 098 d.setDistance(0); 099 result = false; 100 } 101 break; 102 } 103 } 104 return result; 105 } 106 107 boolean navigateTrackSegment() { 108 boolean result = false; 109 // LayoutTrack block and reported block must be equal 110 if (use_blocks && ((TrackSegment) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) { 111 // not in the block 112 d.setDistance(0); 113 return result; 114 } 115 116 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 117 d.nextLayoutTrack = null; 118 119 TrackSegmentView tsv = d.getModels().getTrackSegmentView((TrackSegment) d.getLayoutTrack()); 120 if (tsv.isArc()) { 121 // tsv.calculateTrackSegmentAngle(); // ... has protected access in TrackSegmentView 122 // when do we need this? After a panel change? 123 Point2D radius2D = new Point2D.Double(tsv.getCW() / 2, tsv.getCH() / 2); 124 double radius = (radius2D.getX() + radius2D.getY()) / 2; 125 Point2D centre = tsv.getCentre(); 126 /* 127 * Note: Angles go CCW from south to east to north to west, etc. 128 * For JMRI angles subtract from 90 to get east, south, west, north 129 */ 130 //double startAdjDEG = tsv.getStartAdj(); // klk The value of the local variable startAdjDEG is not really used 131 double tmpAngleDEG = tsv.getTmpAngle(); 132 133 double distance = 2 * radius * Math.PI * tmpAngleDEG / 360; 134 d.setReturnDistance(distance); 135 if (distanceOnTrack < distance) { // it's on this track 136 Point2D p1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1()); 137 Point2D p2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2()); 138 if (!tsv.isCircle()) { 139 centre = MathUtil.midPoint(p1, p2); 140 Point2D centreSeg = tsv.getCentreSeg(); 141 double newX = (centre.getX() < centreSeg.getX()) ? Math.min(p1.getX(), p2.getX()) : Math.max(p1.getX(), p2.getX()); 142 double newY = (centre.getY() < centreSeg.getY()) ? Math.min(p1.getY(), p2.getY()) : Math.max(p1.getY(), p2.getY()); 143 centre = new Point2D.Double(newX, newY); 144 } 145 double angle1DEG = MathUtil.computeAngleDEG(p1, centre) - 90; 146 double angle2DEG = MathUtil.computeAngleDEG(p2, centre) - 90; 147 Point2D centreSeg = tsv.getCentreSeg(); 148 double angle3DEG = MathUtil.computeAngleDEG(centreSeg, centre) - 90; 149 double angleDeltaDEG = MathUtil.wrapPM360(2 * (angle3DEG - angle1DEG)); 150 double ratio = distanceOnTrack / distance; 151 Point2D delta = new Point2D.Double(radius, 0); 152 double angleDEG = 0; 153 if (tsv.getConnect1().equals(d.getLastTrack())) { 154 // entering from this end... 155 d.nextLayoutTrack = tsv.getConnect2(); 156 d.setReturnLastTrack(tsv.getConnect2()); 157 angleDEG = angle1DEG; 158 angleDeltaDEG = MathUtil.lerp(0, angleDeltaDEG, ratio); 159 } else if (tsv.getConnect2().equals(d.getLastTrack())) { 160 // entering from the other end... 161 d.nextLayoutTrack = tsv.getConnect1(); 162 d.setReturnLastTrack(tsv.getConnect1()); 163 //startAdjDEG += tmpAngleDEG; // SpotBugs: Dead store to startAdjDEG 164 angleDEG = angle2DEG; 165 angleDeltaDEG = MathUtil.lerp(0, -angleDeltaDEG, ratio); 166 } else { // OOPS! we're lost! 167 log.info(" lost"); 168 result = false; 169 angleDeltaDEG = 0; 170 } 171 double dirDeltaDEG = Math.signum(angleDeltaDEG) * -90; 172 173 double newAngleDeg = -(angleDEG + angleDeltaDEG); 174 // Compute location 175 delta = MathUtil.rotateDEG(delta, newAngleDeg); 176 if (!tsv.isCircle()) { 177 delta = MathUtil.multiply(delta, radius2D.getX() / radius, radius2D.getY() / radius); 178 } 179 d.setLocation(MathUtil.add(centre, delta)); 180 d.setDirectionDEG(newAngleDeg + dirDeltaDEG); 181 d.setDistance(0); 182 } else { // it's not on this track 183 d.nextLayoutTrack = tsv.getConnect2(); 184 if (tsv.getConnect2().equals(d.getLastTrack())) { 185 // entering from the other end... 186 d.nextLayoutTrack = tsv.getConnect1(); 187 } 188 d.setDistance(distanceOnTrack - distance); 189 distanceOnTrack = 0; 190 result = true; 191 } 192 d.distanceOnTrack = distanceOnTrack; 193 } else if (tsv.isBezier()) { 194 //Point2D[] points = tsv.getBezierPoints(); // getBezierPoints() has private access in TrackSegmentView! 195 // Alternative 196 Point2D ep1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1()); 197 Point2D ep2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2()); 198 int cnt = tsv.getBezierControlPoints().size() + 2; 199 Point2D[] points = new Point2D[cnt]; 200 points[0] = ep1; 201 for (int idx = 0; idx < cnt - 2; idx++) { 202 points[idx + 1] = tsv.getBezierControlPoints().get(idx); 203 } 204 points[cnt - 1] = ep2; 205 206 double distance = MathUtil.drawBezier(null, points); 207 d.setReturnDistance(distance); 208 if (distanceOnTrack < distance) { // it's on this track 209 d.nextLayoutTrack = tsv.getConnect2(); 210 d.setReturnLastTrack(tsv.getConnect2()); 211 // if entering from the other end... 212 if (tsv.getConnect2().equals(d.getLastTrack())) { 213 points = jmri.util.ArrayUtil.reverse(points); //..reverse the points 214 d.nextLayoutTrack = tsv.getConnect1(); // and change the next LayoutTrack 215 d.setReturnLastTrack(tsv.getConnect1()); 216 } 217 GeneralPath path = MathUtil.getBezierPath(points); 218 PathIterator i = path.getPathIterator(null); 219 List<Point2D> pathPoints = new ArrayList<>(); 220 while (!i.isDone()) { 221 float[] data = new float[6]; 222 switch (i.currentSegment(data)) { 223 case PathIterator.SEG_MOVETO: 224 case PathIterator.SEG_LINETO: { 225 pathPoints.add(new Point2D.Double(data[0], data[1])); 226 break; 227 } 228 default: { 229 log.error("Unknown path segment type: {}.", i.currentSegment(data)); 230 //$FALL-THROUGH$ 231 // case PathIterator.SEG_QUADTO: 232 // case PathIterator.SEG_CUBICTO: 233 // case PathIterator.SEG_CLOSE: { 234 // OOPS! we're lost! 235 log.info(" bezier lost"); 236 result = false; 237 break; 238 } 239 } 240 i.next(); 241 } // while (!i.isDone()) 242 return navigate(pathPoints, d.nextLayoutTrack); 243 } else { // it's not on this track 244 d.nextLayoutTrack = tsv.getConnect2(); 245 if (tsv.getConnect2().equals(d.getLastTrack())) { 246 d.nextLayoutTrack = tsv.getConnect1(); 247 } 248 d.setDistance(distanceOnTrack - distance); 249 distanceOnTrack = 0; 250 result = true; 251 } 252 d.distanceOnTrack = distanceOnTrack; 253 } else { 254 Point2D p1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1()); 255 Point2D p2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2()); 256 double distance = MathUtil.distance(p1, p2); 257 d.setReturnDistance(distance); 258 if (distanceOnTrack < distance) { 259 // it's on this track 260 if (tsv.getConnect1().equals(d.getLastTrack())) { 261 d.nextLayoutTrack = tsv.getConnect2(); 262 d.setReturnLastTrack(tsv.getConnect2()); 263 } else if (tsv.getConnect2().equals(d.getLastTrack())) { 264 // if entering from the other end then swap end points 265 d.nextLayoutTrack = tsv.getConnect1(); 266 d.setReturnLastTrack(tsv.getConnect1()); 267 // swap 268 Point2D temp = p1; 269 p1 = p2; 270 p2 = temp; 271 } else { // OOPS! we're lost! 272 result = false; 273 } 274 double ratio = distanceOnTrack / distance; 275 d.setLocation(MathUtil.lerp(p1, p2, ratio)); 276 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p2, p1)); 277 d.setDistance(0); 278 } else { // it's not on this track 279 if (tsv.getConnect1().equals(d.getLastTrack())) { 280 d.nextLayoutTrack = tsv.getConnect2(); 281 } else if (tsv.getConnect2().equals(d.getLastTrack())) { 282 d.nextLayoutTrack = tsv.getConnect1(); 283 } 284 d.setDistance(distanceOnTrack - distance); 285 distanceOnTrack = 0; 286 result = true; 287 } 288 d.distanceOnTrack = distanceOnTrack; 289 } 290 291 if (result) { // not on this track 292 // go to next track 293 LayoutTrack last = d.getLayoutTrack(); 294 if (d.nextLayoutTrack != null) { 295 d.setLayoutTrack(d.nextLayoutTrack); 296 } else { // OOPS! we're lost! 297 result = false; 298 } 299 if (result) { 300 d.setLastTrack(last); 301 d.setReturnTrack(d.getLayoutTrack()); 302 d.setReturnLastTrack(d.getLayoutTrack()); 303 } 304 } 305 d.setTunnelState(tsv.isTunnelSideRight() || tsv.isTunnelSideLeft() || tsv.isTunnelHasEntry() 306 || tsv.isTunnelHasExit() ? true : false); 307 return result; 308 } 309 310 boolean navigateLayoutTurnout() { 311 boolean result = false; 312 if (use_blocks && ((LayoutTurnout) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) { 313 // we are not in the block 314 d.setDistance(0); 315 return result; 316 } 317 318 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 319 320 LayoutTurnoutView tv = d.getModels().getLayoutTurnoutView((LayoutTurnout) d.getLayoutTrack()); 321 Point2D pM = tv.getCoordsCenter(); 322 Point2D pA = tv.getCoordsA(); 323 Point2D pB = tv.getCoordsB(); 324 Point2D pC = tv.getCoordsC(); 325 Point2D pD = tv.getCoordsD(); 326 327 int state = LayoutTurnout.UNKNOWN; // 1 328 if (d.getModels().isAnimating()) { 329 state = tv.getState(); // turnout closed: 2, turnout thrown: 4 330 } 331 if ((state != jmri.Turnout.CLOSED) && (state != jmri.Turnout.THROWN)) { 332 log.info("have to stop - state: {}", state); // state UNKNOWN 333 result = false; 334 } 335 336 d.nextLayoutTrack = null; 337 338 switch (tv.getTurnoutType()) { 339 case RH_TURNOUT: 340 case LH_TURNOUT: 341 case WYE_TURNOUT: { 342 Point2D pStart = null; 343 Point2D pEnd = null; 344 345 if (tv.getConnectA().equals(d.getLastTrack())) { 346 pStart = pA; 347 if (state == jmri.Turnout.CLOSED) { 348 pEnd = pB; 349 d.nextLayoutTrack = tv.getConnectB(); 350 } else if (state == jmri.Turnout.THROWN) { 351 pEnd = pC; 352 d.nextLayoutTrack = tv.getConnectC(); 353 } 354 } else if (tv.getConnectB().equals(d.getLastTrack())) { 355 if (state == jmri.Turnout.CLOSED) { 356 pStart = pB; 357 pEnd = pA; 358 d.nextLayoutTrack = tv.getConnectA(); 359 } 360 } else if (tv.getConnectC().equals(d.getLastTrack())) { 361 if (state == jmri.Turnout.THROWN) { 362 pStart = pC; 363 pEnd = pA; 364 d.nextLayoutTrack = tv.getConnectA(); 365 } 366 } else { // OOPS! we're lost! 367 result = false; 368 } 369 if (d.nextLayoutTrack != null) { 370 d.setReturnLastTrack(d.nextLayoutTrack); 371 d.setReturnTrack(d.getLayoutTrack()); 372 d.setDistance(0); 373 } 374 375 if (pStart != null) { 376 double distanceStart = MathUtil.distance(pStart, pM); 377 d.setReturnDistance(distanceStart); 378 if (distanceOnTrack < distanceStart) { // it's on startleg 379 double ratio = distanceOnTrack / distanceStart; 380 d.setLocation(MathUtil.lerp(pStart, pM, ratio)); 381 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pM, pStart)); 382 d.setDistance(0); 383 } else if (pEnd != null) { // it's not on startleg 384 double distanceEnd = MathUtil.distance(pM, pEnd); 385 d.setReturnDistance(distanceEnd); 386 if ((distanceOnTrack - distanceStart) < distanceEnd) { // it's on end leg 387 double ratio = (distanceOnTrack - distanceStart) / distanceEnd; 388 d.setLocation(MathUtil.lerp(pM, pEnd, ratio)); 389 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pEnd, pM)); 390 d.setDistance(0); 391 } else { // it's not on end leg / this track 392 d.setDistance(distanceOnTrack - (distanceStart + distanceEnd)); 393 distanceOnTrack = 0; 394 result = true; 395 } 396 } else { // OOPS! we're lost! 397 log.info(" Turnout has unknown state"); 398 result = false; 399 distanceOnTrack = distanceStart; 400 d.setDistance(0); 401 d.setReturnDistance(0); 402 d.setReturnTrack(d.getLastTrack()); 403 } 404 } else { // OOPS! we're lost! 405 log.info(" Turnout caused a stop"); // correct position or change direction 406 result = false; 407 distanceOnTrack = 0; 408 d.setDistance(0); 409 d.setReturnDistance(0); 410 d.setReturnTrack(d.getLastTrack()); 411 } 412 break; 413 } 414 415 case RH_XOVER: 416 case LH_XOVER: 417 case DOUBLE_XOVER: { 418 List<Point2D> points = new ArrayList<>(); 419 420 // middles 421 Point2D pABM = MathUtil.midPoint(pA, pB); 422 Point2D pAM = pABM, pBM = pABM; 423 424 Point2D pCDM = MathUtil.midPoint(pC, pD); 425 Point2D pCM = pCDM, pDM = pCDM; 426 427 if (tv.getTurnoutType() == LayoutTurnout.TurnoutType.DOUBLE_XOVER) { 428 pAM = MathUtil.lerp(pA, pABM, 5.0 / 8.0); 429 pBM = MathUtil.lerp(pB, pABM, 5.0 / 8.0); 430 pCM = MathUtil.lerp(pC, pCDM, 5.0 / 8.0); 431 pDM = MathUtil.lerp(pD, pCDM, 5.0 / 8.0); 432 } 433 434 if (tv.getConnectA().equals(d.getLastTrack())) { 435 if (state == jmri.Turnout.CLOSED) { 436 points.add(pA); 437 points.add(pB); 438 d.nextLayoutTrack = tv.getConnectB(); 439 } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.LH_XOVER) && (state == jmri.Turnout.THROWN)) { 440 points.add(pA); 441 points.add(pAM); 442 points.add(pCM); 443 points.add(pC); 444 d.nextLayoutTrack = tv.getConnectC(); 445 } 446 } else if (tv.getConnectB().equals(d.getLastTrack())) { 447 if (state == jmri.Turnout.CLOSED) { 448 points.add(pB); 449 points.add(pA); 450 d.nextLayoutTrack = tv.getConnectA(); 451 } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.RH_XOVER) && (state == jmri.Turnout.THROWN)) { 452 points.add(pB); 453 points.add(pBM); 454 points.add(pDM); 455 points.add(pD); 456 d.nextLayoutTrack = tv.getConnectD(); 457 } 458 } else if (tv.getConnectC().equals(d.getLastTrack())) { 459 if (state == jmri.Turnout.CLOSED) { 460 points.add(pC); 461 points.add(pD); 462 d.nextLayoutTrack = tv.getConnectD(); 463 } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.LH_XOVER) && (state == jmri.Turnout.THROWN)) { 464 points.add(pC); 465 points.add(pCM); 466 points.add(pAM); 467 points.add(pA); 468 d.nextLayoutTrack = tv.getConnectA(); 469 } 470 } else if (tv.getConnectD().equals(d.getLastTrack())) { 471 if (state == jmri.Turnout.CLOSED) { 472 points.add(pD); 473 points.add(pC); 474 d.nextLayoutTrack = tv.getConnectC(); 475 } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.RH_XOVER) && (state == jmri.Turnout.THROWN)) { 476 points.add(pD); 477 points.add(pDM); 478 points.add(pBM); 479 points.add(pB); 480 d.nextLayoutTrack = tv.getConnectB(); 481 } 482 } else { // OOPS! we're lost! 483 result = false; 484 } 485 486 if (d.nextLayoutTrack != null) { 487 d.setReturnLastTrack(d.nextLayoutTrack); 488 d.setReturnTrack(d.getLayoutTrack()); 489 } 490 return navigate(points, d.nextLayoutTrack); 491 } 492 493 case SINGLE_SLIP: 494 case DOUBLE_SLIP: { 495 log.warn("TurnoutView {}.navigate(...); slips should be being handled by LayoutSlip sub-class", tv.getName()); 496 break; 497 } 498 default: { // OOPS! we're lost! 499 result = false; 500 break; 501 } 502 } 503 d.distanceOnTrack = distanceOnTrack; 504 505 if (result) { // not on this track 506 // go to next track 507 LayoutTrack last = d.getLayoutTrack(); 508 if (d.nextLayoutTrack != null) { 509 d.setLayoutTrack(d.nextLayoutTrack); 510 } else { // OOPS! we're lost! 511 result = false; 512 } 513 if (result) { 514 d.setLastTrack(last); 515 d.setReturnTrack(d.getLayoutTrack()); 516 d.setReturnLastTrack(d.getLayoutTrack()); 517 } 518 } 519 return result; 520 } 521 522 // NOTE: LayoutSlip uses the checkForNonContiguousBlocks 523 // and collectContiguousTracksNamesInBlockNamed methods 524 // inherited from LayoutTurnout 525 boolean navigateLayoutSlip() { 526 if (use_blocks && ((LayoutSlip) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) { 527 // we are not in the block 528 d.setDistance(0); 529 return false; 530 } 531 532 boolean result = true; // assume success (optimist!) 533 534 LayoutSlipView ltv = d.getModels().getLayoutSlipView((LayoutSlip) d.getLayoutTrack()); 535 536 Point2D pA = ltv.getCoordsA(); 537 Point2D pB = ltv.getCoordsB(); 538 Point2D pC = ltv.getCoordsC(); 539 Point2D pD = ltv.getCoordsD(); 540 541 d.nextLayoutTrack = null; 542 543 List<Point2D> points = new ArrayList<>(); 544 545 // thirds 546 double third = 1.0 / 3.0; 547 Point2D pACT = MathUtil.lerp(pA, pC, third); 548 Point2D pBDT = MathUtil.lerp(pB, pD, third); 549 Point2D pCAT = MathUtil.lerp(pC, pA, third); 550 Point2D pDBT = MathUtil.lerp(pD, pB, third); 551 552 int slipState = ltv.getSlipState(); 553 554 boolean slip_lost = false; 555 556 if (ltv.getConnectA().equals(d.getLastTrack())) { 557 if (slipState == LayoutTurnout.STATE_AC) { 558 points.add(pA); 559 points.add(pC); 560 d.nextLayoutTrack = ltv.getConnectC(); 561 } else if (slipState == LayoutTurnout.STATE_AD) { 562 points.add(pA); 563 points.add(pACT); 564 points.add(pDBT); 565 points.add(pD); 566 d.nextLayoutTrack = ltv.getConnectD(); 567 } else { // OOPS! we're lost! 568 result = false; 569 slip_lost = true; 570 } 571 } else if (ltv.getConnectB().equals(d.getLastTrack())) { 572 if (slipState == LayoutTurnout.STATE_BD) { 573 points.add(pB); 574 points.add(pD); 575 d.nextLayoutTrack = ltv.getConnectD(); 576 } else if (slipState == LayoutTurnout.STATE_BC) { 577 points.add(pB); 578 points.add(pBDT); 579 points.add(pCAT); 580 points.add(pC); 581 d.nextLayoutTrack = ltv.getConnectC(); 582 } else { // OOPS! we're lost! 583 result = false; 584 slip_lost = true; 585 } 586 } else if (ltv.getConnectC().equals(d.getLastTrack())) { 587 if (slipState == LayoutTurnout.STATE_AC) { 588 points.add(pC); 589 points.add(pA); 590 d.nextLayoutTrack = ltv.getConnectA(); 591 } else if (slipState == LayoutTurnout.STATE_BC) { 592 points.add(pC); 593 points.add(pCAT); 594 points.add(pBDT); 595 points.add(pB); 596 d.nextLayoutTrack = ltv.getConnectB(); 597 } else { // OOPS! we're lost! 598 result = false; 599 slip_lost = true; 600 } 601 } else if (ltv.getConnectD().equals(d.getLastTrack())) { 602 if (slipState == LayoutTurnout.STATE_BD) { 603 points.add(pD); 604 points.add(pB); 605 d.nextLayoutTrack = ltv.getConnectB(); 606 } else if (slipState == LayoutTurnout.STATE_AD) { 607 points.add(pD); 608 points.add(pDBT); 609 points.add(pACT); 610 points.add(pA); 611 d.nextLayoutTrack = ltv.getConnectA(); 612 } else { // OOPS! we're lost! 613 result = false; 614 slip_lost = true; 615 } 616 } else { // OOPS! we're lost! 617 result = false; 618 } 619 if (d.nextLayoutTrack != null) { 620 d.setReturnLastTrack(d.nextLayoutTrack); 621 d.setReturnTrack(d.getLayoutTrack()); 622 } 623 if (slip_lost) { 624 log.info(" Turnout state not good"); 625 d.setDistance(0); 626 d.setReturnDistance(0); 627 } 628 629 if (result) { 630 result = navigate(points, d.nextLayoutTrack); 631 } 632 return result; 633 } 634 635 boolean navigateLevelXing() { 636 boolean result = false; 637 jmri.Block block2 = null; 638 LevelXing lx = (LevelXing) d.getLayoutTrack(); 639 if (lx.getConnectA().equals(d.getLastTrack()) || lx.getConnectC().equals(d.getLastTrack())) { 640 block2 = lx.getLayoutBlockAC().getBlock(); 641 } else if (lx.getConnectB().equals(d.getLastTrack()) || lx.getConnectD().equals(d.getLastTrack())) { 642 block2 = lx.getLayoutBlockBD().getBlock(); 643 } 644 if (use_blocks && block2 != VSDecoderManager.instance().currentBlock.get(d)) { 645 // not in the block (blocks do not match) 646 d.setDistance(0); 647 return result; 648 } 649 650 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 651 652 LevelXingView lxv = d.getModels().getLevelXingView((LevelXing) d.getLayoutTrack()); 653 Point2D pA = lxv.getCoordsA(); 654 Point2D pB = lxv.getCoordsB(); 655 Point2D pC = lxv.getCoordsC(); 656 Point2D pD = lxv.getCoordsD(); 657 Point2D p1 = null; 658 Point2D p2 = null; 659 660 d.nextLayoutTrack = null; 661 662 if (lxv.getConnectA().equals(d.getLastTrack())) { 663 p1 = pA; 664 p2 = pC; 665 d.nextLayoutTrack = lxv.getConnectC(); 666 } else if (lxv.getConnectB().equals(d.getLastTrack())) { 667 p1 = pB; 668 p2 = pD; 669 d.nextLayoutTrack = lxv.getConnectD(); 670 } else if (lxv.getConnectC().equals(d.getLastTrack())) { 671 p1 = pC; 672 p2 = pA; 673 d.nextLayoutTrack = lxv.getConnectA(); 674 } else if (lxv.getConnectD().equals(d.getLastTrack())) { 675 p1 = pD; 676 p2 = pB; 677 d.nextLayoutTrack = lxv.getConnectB(); 678 result = false; 679 } 680 if (d.nextLayoutTrack != null) { 681 d.setReturnLastTrack(d.nextLayoutTrack); 682 d.setReturnTrack(d.getLayoutTrack()); 683 } 684 685 if (p1 != null) { 686 double distance = MathUtil.distance(p1, p2); 687 d.setReturnDistance(distance); 688 if (distanceOnTrack < distance) { 689 // it's on this track 690 double ratio = distanceOnTrack / distance; 691 d.setLocation(MathUtil.lerp(p1, p2, ratio)); 692 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p2, p1)); 693 d.setDistance(0); 694 } else { // it's not on this track 695 d.setDistance(distanceOnTrack - distance); 696 distanceOnTrack = 0; 697 result = true; 698 } 699 d.distanceOnTrack = distanceOnTrack; 700 } 701 702 if (result) { // not on this track 703 // go to next track 704 LayoutTrack last = d.getLayoutTrack(); 705 if (d.nextLayoutTrack != null) { 706 d.setLayoutTrack(d.nextLayoutTrack); 707 } else { // OOPS! we're lost! 708 result = false; 709 } 710 if (result) { 711 d.setLastTrack(last); 712 d.setReturnTrack(d.getLayoutTrack()); 713 d.setReturnLastTrack(d.getLayoutTrack()); 714 } 715 } 716 return result; 717 } 718 719 boolean navigateLayoutTurntable() { 720 if (use_blocks && !((LayoutTurntable) d.getLayoutTrack()).getBlockName().equals(VSDecoderManager.instance().currentBlock.get(d).getUserName())) { 721 // we are not in the block 722 d.setDistance(0); 723 return false; 724 } 725 726 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 727 d.nextLayoutTrack = null; 728 729 LayoutTurntable turntable = (LayoutTurntable) d.getLayoutTrack(); 730 LayoutTurntableView ttv = d.getModels().getLayoutTurntableView(turntable); 731 int num_rays = turntable.getNumberRays(); 732 log.debug("turntable name: {}, number rays: {}", ttv.getName(), num_rays); 733 734 Point2D pStart = null; 735 Point2D pEnd = null; 736 737 // some checks ... 738 if (num_rays < 2) { 739 // A turntable must have a ray track at a 180 degree offset for each ray track 740 log.warn("A turntable must have at least two ray tracks)"); 741 } else if (num_rays %2 != 0) { 742 log.warn("A turntable must have an even number of rays"); 743 } else if (turntable.getPosition() < 0) { 744 log.warn("Turntable position not set, pos: {}", turntable.getPosition()); // setting the correct position allows to continue 745 } else { 746 int currentPosition = turntable.getPosition(); 747 int entryRay = -1; 748 int exitRay = -1; 749 if (lastTurntablePosition != -1 && lastTurntablePosition != currentPosition) { 750 // new bridge position detected 751 List<Double> angles = new ArrayList<>(); 752 turntable.getRayTrackList().forEach((rt) -> angles.add(rt.getAngle())); 753 entryRay = angles.indexOf(MathUtil.wrap360(angles.get(currentPosition) + 180.0)); 754 if (entryRay != -1) { 755 d.setLastTrack(turntable.getRayConnectOrdered(entryRay)); 756 d.nextLayoutTrack = turntable.getRayConnectIndexed(currentPosition); 757 } else { 758 log.warn("Counter ray for ray track angle {} not found", angles.get(currentPosition)); 759 } 760 } else { 761 for (int i = 0; i < num_rays; i++) { 762 if (turntable.getRayConnectOrdered(i) == d.getLastTrack()) { 763 entryRay = i; 764 break; 765 } 766 } 767 } 768 exitRay = turntable.getPosition(); 769 770 if (entryRay >= 0 && entryRay < num_rays && entryRay != exitRay) { 771 log.debug("entryray: {}, exitray: {}, current position: {}", entryRay, exitRay, currentPosition); 772 if (exitRay == currentPosition) { 773 pStart = ttv.getRayCoordsIndexed(entryRay); 774 pEnd = ttv.getRayCoordsIndexed(currentPosition); 775 d.setLastTrack(turntable.getRayConnectIndexed(entryRay)); 776 d.nextLayoutTrack = turntable.getRayConnectIndexed(exitRay); 777 lastTurntablePosition = turntable.getPosition(); 778 log.debug("Next layout track set to: {}, last track: {}", d.nextLayoutTrack, d.getLastTrack()); 779 } else { 780 log.warn("Turntable not aligned for exit. Current pos: {}, required for exit ray {}: {}", currentPosition, exitRay, turntable.getRayIndex(exitRay)); 781 } 782 } 783 } 784 785 return navigateComplexTrack(pStart, pEnd, distanceOnTrack); 786 } 787 788 boolean navigateLayoutTraverser() { 789 if (use_blocks && !((LayoutTraverser) d.getLayoutTrack()).getBlockName().equals(VSDecoderManager.instance().currentBlock.get(d).getUserName())) { 790 // we are not in the block 791 d.setDistance(0); 792 return false; 793 } 794 795 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 796 d.nextLayoutTrack = null; 797 798 LayoutTraverser traverser = (LayoutTraverser) d.getLayoutTrack(); 799 LayoutTraverserView trv = d.getModels().getLayoutTraverserView(traverser); 800 int num_slots = traverser.getNumberSlots(); 801 log.debug("traverser name: {}, number slots: {}", trv.getName(), num_slots); 802 803 Point2D pStart = null; 804 Point2D pEnd = null; 805 806 if (num_slots < 1) { 807 log.warn("A traverser must have at least one slot"); 808 } else if (traverser.getPosition() < 0) { 809 log.warn("Traverser position not set, pos: {}", traverser.getPosition()); 810 } else { 811 int currentPosition = traverser.getPosition(); 812 int entrySlot = -1; 813 for (int i = 0; i < num_slots; i++) { 814 if (traverser.getSlotConnectOrdered(i) == d.getLastTrack()) { 815 entrySlot = i; 816 break; 817 } 818 } 819 820 if (entrySlot != -1) { 821 // Find the corresponding exit slot. Traverser slots are in pairs. 822 int exitSlot = (entrySlot % 2 == 0) ? entrySlot + 1 : entrySlot - 1; 823 log.debug("entryslot: {}, exitslot: {}, current position: {}", entrySlot, exitSlot, currentPosition); 824 825 if (exitSlot >= 0 && exitSlot < num_slots) { 826 // Check if the deck is aligned to the exit slot 827 if (traverser.getSlotIndex(exitSlot) == currentPosition) { 828 pStart = trv.getSlotCoordsIndexed(entrySlot); 829 pEnd = trv.getSlotCoordsIndexed(exitSlot); 830 d.nextLayoutTrack = traverser.getSlotConnectOrdered(exitSlot); 831 } else if (exitSlot %2 == currentPosition %2) { 832 // Deck position has moved up or down 833 if (entrySlot < exitSlot) { 834 pStart = trv.getSlotCoordsIndexed(currentPosition - 1); 835 } else { 836 pStart = trv.getSlotCoordsIndexed(currentPosition + 1); 837 } 838 pEnd = trv.getSlotCoordsIndexed(currentPosition); 839 d.nextLayoutTrack = traverser.getSlotConnectOrdered(currentPosition); 840 log.debug("next track: {}", d.nextLayoutTrack); 841 } else { 842 log.warn("Traverser not aligned for exit. Current pos: {}, required for exit slot {}: {}", currentPosition, exitSlot, traverser.getSlotIndex(exitSlot)); 843 } 844 845 } 846 } 847 } 848 849 return navigateComplexTrack(pStart, pEnd, distanceOnTrack); 850 } 851 852 /** 853 * Common navigation logic for complex tracks like Turntables and Traversers. 854 * 855 * @param pStart The starting point of movement on the component. 856 * @param pEnd The ending point of movement on the component. 857 * @param distanceOnTrack The total distance to travel. 858 * @return True if navigation should continue to the next track piece. 859 */ 860 private boolean navigateComplexTrack(Point2D pStart, Point2D pEnd, double distanceOnTrack) { 861 boolean result = false; 862 String tType = ""; 863 if (d.getLayoutTrack() instanceof LayoutTraverser) { 864 tType = "Traverser"; 865 } else { 866 tType = "Turntable"; 867 } 868 869 if (d.nextLayoutTrack != null) { 870 d.setReturnLastTrack(d.nextLayoutTrack); 871 d.setReturnTrack(d.getLayoutTrack()); // just in case of a direction change 872 d.setDistance(0); 873 } 874 875 if (pStart != null && pEnd != null) { 876 double distance = MathUtil.distance(pStart, pEnd); 877 d.setReturnDistance(distance); 878 if (distanceOnTrack < distance) { 879 // it's on this track 880 double ratio = distanceOnTrack / distance; 881 d.setLocation(MathUtil.lerp(pStart, pEnd, ratio)); 882 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pEnd, pStart)); 883 d.setDistance(0); 884 } else { // it's not on this track 885 d.setDistance(distanceOnTrack - distance); 886 distanceOnTrack = 0; 887 result = true; // move to next track 888 } 889 } else { // OOPS! we're lost! 890 log.info("A {} setting caused a stop", tType); // correct position or change direction 891 result = false; 892 distanceOnTrack = 0; 893 d.setDistance(0); 894 d.setReturnDistance(0); 895 d.setReturnTrack(d.getLastTrack()); 896 log.debug("new d.distanceOnTrack: {}, distanceOnTrack: {}, last: {}", d.distanceOnTrack, distanceOnTrack, d.getLastTrack()); 897 } 898 d.distanceOnTrack = distanceOnTrack; 899 900 if (result) { // not on this track 901 // go to next track 902 log.debug("go to next layout track: {}", d.nextLayoutTrack); 903 LayoutTrack last = d.getLayoutTrack(); 904 if (d.nextLayoutTrack != null) { 905 d.setLayoutTrack(d.nextLayoutTrack); 906 if (tType.equals("Turntable")) lastTurntablePosition = -1; 907 } else { 908 log.info("Lost a {} result", tType); 909 result = false; 910 } 911 if (result) { 912 d.setLastTrack(last); 913 d.setReturnTrack(d.getLayoutTrack()); 914 d.setReturnLastTrack(d.getLayoutTrack()); 915 } 916 } 917 return result; 918 } 919 920 private boolean navigate(List<Point2D> points, @CheckForNull LayoutTrack nextLayoutTrack) { 921 boolean result = false; 922 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 923 boolean nextLegFlag = true; 924 Point2D lastPoint = null; 925 double trackDistance = 0; 926 for (Point2D p : points) { 927 if (lastPoint != null) { 928 double distance = MathUtil.distance(lastPoint, p); 929 trackDistance += distance; 930 if (distanceOnTrack < trackDistance) { // it's on this leg 931 d.setLocation(MathUtil.lerp(p, lastPoint, (trackDistance - distanceOnTrack) / distance)); 932 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p, lastPoint)); 933 nextLegFlag = false; 934 break; 935 } 936 } 937 lastPoint = p; 938 } 939 if (nextLegFlag) { // it's not on this track 940 d.setDistance(distanceOnTrack - trackDistance); 941 distanceOnTrack = 0; 942 result = true; 943 } else { // it's on this track 944 d.setDistance(0); 945 } 946 d.distanceOnTrack = distanceOnTrack; 947 if (result) { // not on this track 948 // go to next track 949 LayoutTrack last = d.getLayoutTrack(); 950 if (nextLayoutTrack != null) { 951 d.setLayoutTrack(nextLayoutTrack); 952 } else { // OOPS! we're lost! 953 result = false; 954 } 955 if (result) { 956 d.setLastTrack(last); 957 d.setReturnTrack(d.getLayoutTrack()); 958 d.setReturnLastTrack(d.getLayoutTrack()); 959 } 960 } 961 return result; 962 } 963 964 private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(VSDNavigation.class); 965 966}