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