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