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 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 boolean result = false; 721 if (use_blocks && !((LayoutTurntable) d.getLayoutTrack()).getBlockName().equals(VSDecoderManager.instance().currentBlock.get(d).getUserName())) { 722 // we are not in the block 723 d.setDistance(0); 724 return false; 725 } 726 727 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 728 d.nextLayoutTrack = null; 729 730 LayoutTurntable turntable = (LayoutTurntable) d.getLayoutTrack(); 731 LayoutTurntableView ttv = d.getModels().getLayoutTurntableView(turntable); 732 int num_rays = turntable.getNumberRays(); 733 log.debug("turntable name: {}, number rays: {}", ttv.getName(), num_rays); 734 735 Point2D pStart = null; 736 Point2D pEnd = null; 737 738 // some checks ... 739 if (num_rays < 1) { 740 log.warn("A turntable must have at least one ray (better two)"); 741 } else if (turntable.getPosition() < 0) { 742 log.warn("Turntable position not set"); // setting the correct position allows to continue 743 } else { 744 List<Point2D> points = new ArrayList<>(); 745 for (int i = 0; i < num_rays; i++) { 746 points.add(ttv.getRayCoordsOrdered(i)); 747 } 748 749 for (LayoutTurntable.RayTrack rt : turntable.getRayTrackList()) { 750 if (rt.getConnect().equals(d.getLastTrack())) { 751 // is there a counter-ray? If so, get this index 752 double counterAngle = MathUtil.wrap360(rt.getAngle() + 180.0); 753 boolean found = false; 754 int indexT = -1; // init 755 for (LayoutTurntable.RayTrack rta : turntable.getRayTrackList()) { 756 if (counterAngle == rta.getAngle()) { 757 found = true; // yes, counter-ray exists 758 indexT = rta.getConnectionIndex(); 759 break; 760 } 761 } 762 if (!found) { 763 // ray without counter-ray - not supported (there is no HitPoint for the bridge end) 764 if (turntable.getPosition() == rt.getConnectionIndex()) { 765 log.warn("non-existent opposite ray track; please return"); // going reverse works 766 } else { 767 log.warn("Wrong turntable position - please correct or return"); 768 } 769 } else { 770 boolean is_turned = false; 771 int indexH = rt.getConnectionIndex(); 772 if (lastTurntablePosition >= 0 && turntable.getPosition() != lastTurntablePosition) { 773 // new bridge position detected 774 is_turned = true; 775 double newAngle = turntable.getRayTrackList().get(turntable.getPosition()).getAngle(); 776 double lastAngle = MathUtil.wrap360(newAngle + 180.0); 777 boolean found2 = false; 778 for (LayoutTurntable.RayTrack rtb : turntable.getRayTrackList()) { 779 if (lastAngle == rtb.getAngle()) { 780 found2 = true; // yes, counter-ray exists 781 indexH = rtb.getConnectionIndex(); 782 break; 783 } 784 } 785 if (found2) { 786 d.setLastTrack(turntable.getRayConnectIndexed(indexH)); 787 d.nextLayoutTrack = turntable.getRayConnectIndexed(turntable.getPosition()); 788 indexT = turntable.getPosition(); // update index 789 } else { 790 log.info("non-existent opposite ray track)"); 791 } 792 } 793 794 if (turntable.getPosition() == indexT || turntable.getPosition() == indexH) { 795 // turntable position is correct 796 pStart = points.get(indexH); 797 if (is_turned) { 798 pEnd = points.get(turntable.getPosition()); 799 } else { 800 pEnd = points.get(indexT); 801 } 802 d.nextLayoutTrack = turntable.getRayConnectIndexed(indexT); 803 log.debug("Next layout track set to: {}", d.nextLayoutTrack); 804 lastTurntablePosition = turntable.getPosition(); 805 } else { 806 log.warn("Wrong turntable position - please correct position"); 807 } 808 } 809 break; 810 } 811 } 812 } 813 814 if (d.nextLayoutTrack != null) { 815 d.setReturnLastTrack(d.nextLayoutTrack); 816 d.setReturnTrack(d.getLayoutTrack()); // just in case of a direction change 817 d.setDistance(0); 818 } 819 if (d.nextLayoutTrack == null) { 820 log.debug("Next layout track not set"); 821 result = false; 822 } 823 824 if (pStart != null && pEnd != null) { 825 double distance = MathUtil.distance(pStart, pEnd); 826 d.setReturnDistance(distance); 827 if (distanceOnTrack < distance) { 828 // it's on this track 829 double ratio = distanceOnTrack / distance; 830 d.setLocation(MathUtil.lerp(pStart, pEnd, ratio)); 831 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pEnd, pStart)); 832 d.setDistance(0); 833 } else { // it's not on this track 834 d.setDistance(distanceOnTrack - distance); 835 distanceOnTrack = 0; 836 result = true; 837 } 838 } else { // OOPS! we're lost! 839 log.info("Turntable caused a stop"); // correct position or change direction 840 result = false; 841 distanceOnTrack = 0; 842 d.setDistance(0); 843 d.setReturnDistance(0); 844 d.setReturnTrack(d.getLastTrack()); 845 log.debug("new d.distanceOnTrack: {}, distanceOnTrack: {}, last: {}", d.distanceOnTrack, distanceOnTrack, d.getLastTrack()); 846 } 847 d.distanceOnTrack = distanceOnTrack; 848 849 if (result) { // not on this track 850 // go to next track 851 log.debug("go to next layout track: {}", d.nextLayoutTrack); 852 LayoutTrack last = d.getLayoutTrack(); 853 if (d.nextLayoutTrack != null) { 854 d.setLayoutTrack(d.nextLayoutTrack); 855 lastTurntablePosition = -1; 856 } else { // OOPS! we're lost! 857 log.info(" TURNTABLE RESULT lost"); 858 result = false; 859 } 860 if (result) { 861 d.setLastTrack(last); 862 d.setReturnTrack(d.getLayoutTrack()); 863 d.setReturnLastTrack(d.getLayoutTrack()); 864 } 865 } 866 return result; 867 } 868 869 private boolean navigate(List<Point2D> points, @CheckForNull LayoutTrack nextLayoutTrack) { 870 boolean result = false; 871 double distanceOnTrack = d.getDistance() + d.distanceOnTrack; 872 boolean nextLegFlag = true; 873 Point2D lastPoint = null; 874 double trackDistance = 0; 875 for (Point2D p : points) { 876 if (lastPoint != null) { 877 double distance = MathUtil.distance(lastPoint, p); 878 trackDistance += distance; 879 if (distanceOnTrack < trackDistance) { // it's on this leg 880 d.setLocation(MathUtil.lerp(p, lastPoint, (trackDistance - distanceOnTrack) / distance)); 881 d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p, lastPoint)); 882 nextLegFlag = false; 883 break; 884 } 885 } 886 lastPoint = p; 887 } 888 if (nextLegFlag) { // it's not on this track 889 d.setDistance(distanceOnTrack - trackDistance); 890 distanceOnTrack = 0; 891 result = true; 892 } else { // it's on this track 893 d.setDistance(0); 894 } 895 d.distanceOnTrack = distanceOnTrack; 896 if (result) { // not on this track 897 // go to next track 898 LayoutTrack last = d.getLayoutTrack(); 899 if (nextLayoutTrack != null) { 900 d.setLayoutTrack(nextLayoutTrack); 901 } else { // OOPS! we're lost! 902 result = false; 903 } 904 if (result) { 905 d.setLastTrack(last); 906 d.setReturnTrack(d.getLayoutTrack()); 907 d.setReturnLastTrack(d.getLayoutTrack()); 908 } 909 } 910 return result; 911 } 912 913 private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(VSDNavigation.class); 914 915}