From 4efa90fd1366f37a380284cf881c0bc79049c644 Mon Sep 17 00:00:00 2001 From: Smallp Tsai Date: Wed, 10 Oct 2018 21:42:27 +0800 Subject: [PATCH] add run/climb movement --- software/hexapod7697/src/hexapod/movement.cpp | 14 +- software/hexapod7697/src/hexapod/movement.h | 3 + .../hexapod7697/src/hexapod/movement_table.h | 424 +++++++++++++----- software/hexapod7697/src/normal_mode.cpp | 20 +- software/pathTool/src/config.py | 48 ++ software/pathTool/src/kinematics.py | 25 ++ software/pathTool/src/main.py | 86 +++- software/pathTool/src/path/__init__.py | 0 software/pathTool/src/path/backward.py | 4 +- software/pathTool/src/path/climb.py | 27 ++ software/pathTool/src/path/forward.py | 2 +- software/pathTool/src/path/forwardfast.py | 25 ++ software/pathTool/src/path/lib.py | 33 ++ software/pathTool/src/path/shiftleft.py | 2 +- software/pathTool/src/path/shiftright.py | 2 +- software/pathTool/src/path/turnleft.py | 2 +- software/pathTool/src/path/turnright.py | 2 +- software/pathTool/src/path/twist.py | 35 ++ 18 files changed, 607 insertions(+), 147 deletions(-) create mode 100644 software/pathTool/src/config.py create mode 100644 software/pathTool/src/kinematics.py create mode 100644 software/pathTool/src/path/__init__.py create mode 100644 software/pathTool/src/path/climb.py create mode 100644 software/pathTool/src/path/forwardfast.py create mode 100644 software/pathTool/src/path/twist.py diff --git a/software/hexapod7697/src/hexapod/movement.cpp b/software/hexapod7697/src/hexapod/movement.cpp index a05a19f..f291bd0 100644 --- a/software/hexapod7697/src/hexapod/movement.cpp +++ b/software/hexapod7697/src/hexapod/movement.cpp @@ -8,26 +8,32 @@ namespace hexapod { extern const MovementTable& standbyTable(); extern const MovementTable& forwardTable(); + extern const MovementTable& forwardfastTable(); extern const MovementTable& backwardTable(); extern const MovementTable& turnleftTable(); extern const MovementTable& turnrightTable(); extern const MovementTable& shiftleftTable(); extern const MovementTable& shiftrightTable(); + extern const MovementTable& climbTable(); extern const MovementTable& rotatexTable(); extern const MovementTable& rotateyTable(); extern const MovementTable& rotatezTable(); + extern const MovementTable& twistTable(); const MovementTable kTable[MOVEMENT_TOTAL] { standbyTable(), forwardTable(), + forwardfastTable(), backwardTable(), turnleftTable(), turnrightTable(), shiftleftTable(), shiftrightTable(), + climbTable(), rotatexTable(), rotateyTable(), rotatezTable(), + twistTable(), }; Movement::Movement(MovementMode mode): @@ -36,6 +42,12 @@ namespace hexapod { } void Movement::setMode(MovementMode newMode) { + + if (!kTable[newMode].entries) { + LOG_INFO("Error: null movement of mode(%d)!", newMode); + return; + } + mode_ = newMode; const MovementTable& table = kTable[mode_]; @@ -48,7 +60,7 @@ namespace hexapod { const MovementTable& table = kTable[mode_]; - if (elapsed <= 0) + if (elapsed <= 0) elapsed = table.stepDuration; if (remainTime_ <= 0) { diff --git a/software/hexapod7697/src/hexapod/movement.h b/software/hexapod7697/src/hexapod/movement.h index 9b21786..394d395 100644 --- a/software/hexapod7697/src/hexapod/movement.h +++ b/software/hexapod7697/src/hexapod/movement.h @@ -8,14 +8,17 @@ namespace hexapod { MOVEMENT_STANDBY = 0, MOVEMENT_FORWARD, + MOVEMENT_FORWARDFAST, MOVEMENT_BACKWARD, MOVEMENT_TURNLEFT, MOVEMENT_TURNRIGHT, MOVEMENT_SHIFTLEFT, MOVEMENT_SHIFTRIGHT, + MOVEMENT_CLIMB, MOVEMENT_ROTATEX, MOVEMENT_ROTATEY, MOVEMENT_ROTATEZ, + MOVEMENT_TWIST, MOVEMENT_TOTAL, }; diff --git a/software/hexapod7697/src/hexapod/movement_table.h b/software/hexapod7697/src/hexapod/movement_table.h index 64fd475..bc31056 100644 --- a/software/hexapod7697/src/hexapod/movement_table.h +++ b/software/hexapod7697/src/hexapod/movement_table.h @@ -4,55 +4,105 @@ namespace { const Locations backward_paths[] { - {{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}}, - {{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}}, - {{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}}, - {{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}}, - {{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}}, - {{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}}, - {{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}}, - {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}}, + {{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}}, + {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}}, + {{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}}, + {{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}}, + {{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}}, + {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}}, + {{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}}, }; const int backward_entries[] { 0,10 }; const MovementTable backward_table {backward_paths, 20, 20, backward_entries, 2 }; +const Locations climb_paths[] { + {{P1X+(30.00), P1Y+(0.00), P1Z+(50.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(-30.00)}, {P3X+(30.00), P3Y+(0.00), P3Z+(50.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(-30.00)}, {P5X+(-30.00), P5Y+(0.00), P5Z+(50.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(-30.00)}}, + {{P1X+(28.53), P1Y+(6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(-30.00)}}, + {{P1X+(24.27), P1Y+(11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(-30.00)}}, + {{P1X+(17.63), P1Y+(16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(-30.00)}}, + {{P1X+(9.27), P1Y+(19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(-30.00)}}, + {{P1X+(0.00), P1Y+(20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(-30.00)}, {P4X+(-0.00), P4Y+(-20.00), P4Z+(-30.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(-30.00)}, {P6X+(-0.00), P6Y+(-20.00), P6Z+(-30.00)}}, + {{P1X+(0.00), P1Y+(16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(-19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(-19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(-19.02), P6Z+(-5.28)}}, + {{P1X+(0.00), P1Y+(12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(-16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(-16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(-16.18), P6Z+(17.02)}}, + {{P1X+(0.00), P1Y+(8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(-11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(-11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(-11.76), P6Z+(34.72)}}, + {{P1X+(0.00), P1Y+(4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(-6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(-6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(-6.18), P6Z+(46.08)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(-30.00)}, {P2X+(30.00), P2Y+(0.00), P2Z+(50.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(-30.00)}, {P4X+(-30.00), P4Y+(0.00), P4Z+(50.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(-30.00)}, {P6X+(-30.00), P6Y+(0.00), P6Z+(50.00)}}, + {{P1X+(0.00), P1Y+(-4.00), P1Z+(-30.00)}, {P2X+(28.53), P2Y+(6.18), P2Z+(46.08)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(-30.00)}, {P4X+(-28.53), P4Y+(6.18), P4Z+(46.08)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(-30.00)}, {P6X+(-28.53), P6Y+(6.18), P6Z+(46.08)}}, + {{P1X+(0.00), P1Y+(-8.00), P1Z+(-30.00)}, {P2X+(24.27), P2Y+(11.76), P2Z+(34.72)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(-30.00)}, {P4X+(-24.27), P4Y+(11.76), P4Z+(34.72)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(-30.00)}, {P6X+(-24.27), P6Y+(11.76), P6Z+(34.72)}}, + {{P1X+(0.00), P1Y+(-12.00), P1Z+(-30.00)}, {P2X+(17.63), P2Y+(16.18), P2Z+(17.02)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(-30.00)}, {P4X+(-17.63), P4Y+(16.18), P4Z+(17.02)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(-30.00)}, {P6X+(-17.63), P6Y+(16.18), P6Z+(17.02)}}, + {{P1X+(0.00), P1Y+(-16.00), P1Z+(-30.00)}, {P2X+(9.27), P2Y+(19.02), P2Z+(-5.28)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(-30.00)}, {P4X+(-9.27), P4Y+(19.02), P4Z+(-5.28)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(-30.00)}, {P6X+(-9.27), P6Y+(19.02), P6Z+(-5.28)}}, + {{P1X+(0.00), P1Y+(-20.00), P1Z+(-30.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(-30.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(-30.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(-30.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(-30.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(-30.00)}}, + {{P1X+(9.27), P1Y+(-19.02), P1Z+(-5.28)}, {P2X+(0.00), P2Y+(16.00), P2Z+(-30.00)}, {P3X+(9.27), P3Y+(-19.02), P3Z+(-5.28)}, {P4X+(0.00), P4Y+(16.00), P4Z+(-30.00)}, {P5X+(-9.27), P5Y+(-19.02), P5Z+(-5.28)}, {P6X+(0.00), P6Y+(16.00), P6Z+(-30.00)}}, + {{P1X+(17.63), P1Y+(-16.18), P1Z+(17.02)}, {P2X+(0.00), P2Y+(12.00), P2Z+(-30.00)}, {P3X+(17.63), P3Y+(-16.18), P3Z+(17.02)}, {P4X+(0.00), P4Y+(12.00), P4Z+(-30.00)}, {P5X+(-17.63), P5Y+(-16.18), P5Z+(17.02)}, {P6X+(0.00), P6Y+(12.00), P6Z+(-30.00)}}, + {{P1X+(24.27), P1Y+(-11.76), P1Z+(34.72)}, {P2X+(0.00), P2Y+(8.00), P2Z+(-30.00)}, {P3X+(24.27), P3Y+(-11.76), P3Z+(34.72)}, {P4X+(0.00), P4Y+(8.00), P4Z+(-30.00)}, {P5X+(-24.27), P5Y+(-11.76), P5Z+(34.72)}, {P6X+(0.00), P6Y+(8.00), P6Z+(-30.00)}}, + {{P1X+(28.53), P1Y+(-6.18), P1Z+(46.08)}, {P2X+(0.00), P2Y+(4.00), P2Z+(-30.00)}, {P3X+(28.53), P3Y+(-6.18), P3Z+(46.08)}, {P4X+(0.00), P4Y+(4.00), P4Z+(-30.00)}, {P5X+(-28.53), P5Y+(-6.18), P5Z+(46.08)}, {P6X+(0.00), P6Y+(4.00), P6Z+(-30.00)}}, +}; +const int climb_entries[] { 0,10 }; +const MovementTable climb_table {climb_paths, 20, 30, climb_entries, 2 }; + const Locations forward_paths[] { - {{P1X+(0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(-4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(-4.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(-8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(-8.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(-12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(-12.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(-16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(-16.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-19.02), P6Z+(6.18)}}, - {{P1X+(0.00), P1Y+(12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-16.18), P6Z+(11.76)}}, - {{P1X+(0.00), P1Y+(8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-11.76), P6Z+(16.18)}}, - {{P1X+(0.00), P1Y+(4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-6.18), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}}, - {{P1X+(0.00), P1Y+(-4.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(0.00), P3Y+(-4.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(6.18), P4Z+(19.02)}, {P5X+(0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(6.18), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(-8.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(0.00), P3Y+(-8.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(11.76), P4Z+(16.18)}, {P5X+(0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(11.76), P6Z+(16.18)}}, - {{P1X+(0.00), P1Y+(-12.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(0.00), P3Y+(-12.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(16.18), P4Z+(11.76)}, {P5X+(0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(16.18), P6Z+(11.76)}}, - {{P1X+(0.00), P1Y+(-16.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(0.00), P3Y+(-16.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(19.02), P4Z+(6.18)}, {P5X+(0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(19.02), P6Z+(6.18)}}, - {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-19.02), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-19.02), P3Z+(6.18)}, {P4X+(0.00), P4Y+(16.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(0.00), P6Y+(16.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-16.18), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-16.18), P3Z+(11.76)}, {P4X+(0.00), P4Y+(12.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(0.00), P6Y+(12.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-11.76), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-11.76), P3Z+(16.18)}, {P4X+(0.00), P4Y+(8.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(0.00), P6Y+(8.00), P6Z+(0.00)}}, - {{P1X+(0.00), P1Y+(-6.18), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-6.18), P3Z+(19.02)}, {P4X+(0.00), P4Y+(4.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(0.00), P6Y+(4.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(-5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(-5.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(-15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(-15.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-25.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-23.78), P6Z+(7.73)}}, + {{P1X+(0.00), P1Y+(15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-20.23), P6Z+(14.69)}}, + {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-14.69), P6Z+(20.23)}}, + {{P1X+(0.00), P1Y+(5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-7.73), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}}, + {{P1X+(0.00), P1Y+(-5.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(0.00), P3Y+(-5.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(7.73), P4Z+(23.78)}, {P5X+(0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(7.73), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(14.69), P4Z+(20.23)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(14.69), P6Z+(20.23)}}, + {{P1X+(0.00), P1Y+(-15.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(0.00), P3Y+(-15.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(20.23), P4Z+(14.69)}, {P5X+(0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(20.23), P6Z+(14.69)}}, + {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(23.78), P4Z+(7.73)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(23.78), P6Z+(7.73)}}, + {{P1X+(0.00), P1Y+(-25.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-25.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(25.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(25.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-23.78), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-23.78), P3Z+(7.73)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-20.23), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-20.23), P3Z+(14.69)}, {P4X+(0.00), P4Y+(15.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(0.00), P6Y+(15.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-14.69), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-14.69), P3Z+(20.23)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-7.73), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-7.73), P3Z+(23.78)}, {P4X+(0.00), P4Y+(5.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(0.00), P6Y+(5.00), P6Z+(0.00)}}, }; const int forward_entries[] { 0,10 }; const MovementTable forward_table {forward_paths, 20, 20, forward_entries, 2 }; +const Locations forwardfast_paths[] { + {{P1X+(10.00), P1Y+(0.00), P1Z+(30.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(10.00), P3Y+(0.00), P3Z+(30.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(30.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(9.51), P1Y+(15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(-10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(-10.00), P6Z+(0.00)}}, + {{P1X+(8.09), P1Y+(29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(-20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(-20.00), P6Z+(0.00)}}, + {{P1X+(5.88), P1Y+(40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(-30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(-30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(-30.00), P6Z+(0.00)}}, + {{P1X+(3.09), P1Y+(47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(-40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(-40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(-40.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(50.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(-50.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(50.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-50.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(-47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(-47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(-47.55), P6Z+(9.27)}}, + {{P1X+(0.00), P1Y+(30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(-40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(-40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(-40.45), P6Z+(17.63)}}, + {{P1X+(0.00), P1Y+(20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(-29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(-29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(-29.39), P6Z+(24.27)}}, + {{P1X+(0.00), P1Y+(10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(-15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(-15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(-15.45), P6Z+(28.53)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(10.00), P2Y+(0.00), P2Z+(30.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(30.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(30.00)}}, + {{P1X+(0.00), P1Y+(-10.00), P1Z+(0.00)}, {P2X+(9.51), P2Y+(15.45), P2Z+(28.53)}, {P3X+(0.00), P3Y+(-10.00), P3Z+(0.00)}, {P4X+(-9.51), P4Y+(15.45), P4Z+(28.53)}, {P5X+(0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(-9.51), P6Y+(15.45), P6Z+(28.53)}}, + {{P1X+(0.00), P1Y+(-20.00), P1Z+(0.00)}, {P2X+(8.09), P2Y+(29.39), P2Z+(24.27)}, {P3X+(0.00), P3Y+(-20.00), P3Z+(0.00)}, {P4X+(-8.09), P4Y+(29.39), P4Z+(24.27)}, {P5X+(0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(-8.09), P6Y+(29.39), P6Z+(24.27)}}, + {{P1X+(0.00), P1Y+(-30.00), P1Z+(0.00)}, {P2X+(5.88), P2Y+(40.45), P2Z+(17.63)}, {P3X+(0.00), P3Y+(-30.00), P3Z+(0.00)}, {P4X+(-5.88), P4Y+(40.45), P4Z+(17.63)}, {P5X+(0.00), P5Y+(-30.00), P5Z+(0.00)}, {P6X+(-5.88), P6Y+(40.45), P6Z+(17.63)}}, + {{P1X+(0.00), P1Y+(-40.00), P1Z+(0.00)}, {P2X+(3.09), P2Y+(47.55), P2Z+(9.27)}, {P3X+(0.00), P3Y+(-40.00), P3Z+(0.00)}, {P4X+(-3.09), P4Y+(47.55), P4Z+(9.27)}, {P5X+(0.00), P5Y+(-40.00), P5Z+(0.00)}, {P6X+(-3.09), P6Y+(47.55), P6Z+(9.27)}}, + {{P1X+(0.00), P1Y+(-50.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(50.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-50.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(50.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-50.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(50.00), P6Z+(0.00)}}, + {{P1X+(3.09), P1Y+(-47.55), P1Z+(9.27)}, {P2X+(0.00), P2Y+(40.00), P2Z+(0.00)}, {P3X+(3.09), P3Y+(-47.55), P3Z+(9.27)}, {P4X+(0.00), P4Y+(40.00), P4Z+(0.00)}, {P5X+(-3.09), P5Y+(-47.55), P5Z+(9.27)}, {P6X+(0.00), P6Y+(40.00), P6Z+(0.00)}}, + {{P1X+(5.88), P1Y+(-40.45), P1Z+(17.63)}, {P2X+(0.00), P2Y+(30.00), P2Z+(0.00)}, {P3X+(5.88), P3Y+(-40.45), P3Z+(17.63)}, {P4X+(0.00), P4Y+(30.00), P4Z+(0.00)}, {P5X+(-5.88), P5Y+(-40.45), P5Z+(17.63)}, {P6X+(0.00), P6Y+(30.00), P6Z+(0.00)}}, + {{P1X+(8.09), P1Y+(-29.39), P1Z+(24.27)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(8.09), P3Y+(-29.39), P3Z+(24.27)}, {P4X+(0.00), P4Y+(20.00), P4Z+(0.00)}, {P5X+(-8.09), P5Y+(-29.39), P5Z+(24.27)}, {P6X+(0.00), P6Y+(20.00), P6Z+(0.00)}}, + {{P1X+(9.51), P1Y+(-15.45), P1Z+(28.53)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(9.51), P3Y+(-15.45), P3Z+(28.53)}, {P4X+(0.00), P4Y+(10.00), P4Z+(0.00)}, {P5X+(-9.51), P5Y+(-15.45), P5Z+(28.53)}, {P6X+(0.00), P6Y+(10.00), P6Z+(0.00)}}, +}; +const int forwardfast_entries[] { 0,10 }; +const MovementTable forwardfast_table {forwardfast_paths, 20, 20, forwardfast_entries, 2 }; + const Locations rotatex_paths[] { {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*0.97 + P1Z*-0.26 + 0.00, P1X*0.00 + P1Y*0.26 + P1Z*0.97 + 0.00}, {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*0.97 + P2Z*-0.26 + 0.00, P2X*0.00 + P2Y*0.26 + P2Z*0.97 + 0.00}, @@ -429,112 +479,243 @@ const int rotatez_entries[] { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19 const MovementTable rotatez_table {rotatez_paths, 20, 50, rotatez_entries, 20 }; const Locations shiftleft_paths[] { - {{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}}, - {{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}}, - {{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}}, - {{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(20.00)}}, - {{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}}, - {{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}}, - {{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}}, - {{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}}, - {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(-0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}}, + {{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}}, + {{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}}, + {{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(0.00), P6Z+(25.00)}}, + {{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}}, + {{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}}, + {{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}}, + {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}}, + {{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.00), P6Y+(0.00), P6Z+(0.00)}}, }; const int shiftleft_entries[] { 0,10 }; const MovementTable shiftleft_table {shiftleft_paths, 20, 20, shiftleft_entries, 2 }; const Locations shiftright_paths[] { - {{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(6.18), P1Y+(-0.00), P1Z+(19.02)}, {P2X+(-4.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(6.18), P3Y+(-0.00), P3Z+(19.02)}, {P4X+(-4.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(6.18), P5Y+(-0.00), P5Z+(19.02)}, {P6X+(-4.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(11.76), P1Y+(-0.00), P1Z+(16.18)}, {P2X+(-8.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(11.76), P3Y+(-0.00), P3Z+(16.18)}, {P4X+(-8.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(11.76), P5Y+(-0.00), P5Z+(16.18)}, {P6X+(-8.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(16.18), P1Y+(-0.00), P1Z+(11.76)}, {P2X+(-12.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(16.18), P3Y+(-0.00), P3Z+(11.76)}, {P4X+(-12.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(16.18), P5Y+(-0.00), P5Z+(11.76)}, {P6X+(-12.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(19.02), P1Y+(-0.00), P1Z+(6.18)}, {P2X+(-16.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(19.02), P3Y+(-0.00), P3Z+(6.18)}, {P4X+(-16.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(19.02), P5Y+(-0.00), P5Z+(6.18)}, {P6X+(-16.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(16.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-19.02), P2Y+(0.00), P2Z+(6.18)}, {P3X+(16.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-19.02), P4Y+(0.00), P4Z+(6.18)}, {P5X+(16.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-19.02), P6Y+(0.00), P6Z+(6.18)}}, - {{P1X+(12.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-16.18), P2Y+(0.00), P2Z+(11.76)}, {P3X+(12.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-16.18), P4Y+(0.00), P4Z+(11.76)}, {P5X+(12.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-16.18), P6Y+(0.00), P6Z+(11.76)}}, - {{P1X+(8.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-11.76), P2Y+(0.00), P2Z+(16.18)}, {P3X+(8.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-11.76), P4Y+(0.00), P4Z+(16.18)}, {P5X+(8.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-11.76), P6Y+(0.00), P6Z+(16.18)}}, - {{P1X+(4.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-6.18), P2Y+(0.00), P2Z+(19.02)}, {P3X+(4.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-6.18), P4Y+(0.00), P4Z+(19.02)}, {P5X+(4.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-6.18), P6Y+(0.00), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(20.00)}}, - {{P1X+(-4.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(6.18), P2Y+(-0.00), P2Z+(19.02)}, {P3X+(-4.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(6.18), P4Y+(-0.00), P4Z+(19.02)}, {P5X+(-4.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(6.18), P6Y+(-0.00), P6Z+(19.02)}}, - {{P1X+(-8.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(11.76), P2Y+(-0.00), P2Z+(16.18)}, {P3X+(-8.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(11.76), P4Y+(-0.00), P4Z+(16.18)}, {P5X+(-8.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(11.76), P6Y+(-0.00), P6Z+(16.18)}}, - {{P1X+(-12.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(16.18), P2Y+(-0.00), P2Z+(11.76)}, {P3X+(-12.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(16.18), P4Y+(-0.00), P4Z+(11.76)}, {P5X+(-12.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(16.18), P6Y+(-0.00), P6Z+(11.76)}}, - {{P1X+(-16.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(19.02), P2Y+(-0.00), P2Z+(6.18)}, {P3X+(-16.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(19.02), P4Y+(-0.00), P4Z+(6.18)}, {P5X+(-16.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(19.02), P6Y+(-0.00), P6Z+(6.18)}}, - {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-19.02), P1Y+(0.00), P1Z+(6.18)}, {P2X+(16.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-19.02), P3Y+(0.00), P3Z+(6.18)}, {P4X+(16.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-19.02), P5Y+(0.00), P5Z+(6.18)}, {P6X+(16.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-16.18), P1Y+(0.00), P1Z+(11.76)}, {P2X+(12.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-16.18), P3Y+(0.00), P3Z+(11.76)}, {P4X+(12.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-16.18), P5Y+(0.00), P5Z+(11.76)}, {P6X+(12.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-11.76), P1Y+(0.00), P1Z+(16.18)}, {P2X+(8.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-11.76), P3Y+(0.00), P3Z+(16.18)}, {P4X+(8.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-11.76), P5Y+(0.00), P5Z+(16.18)}, {P6X+(8.00), P6Y+(-0.00), P6Z+(0.00)}}, - {{P1X+(-6.18), P1Y+(0.00), P1Z+(19.02)}, {P2X+(4.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-6.18), P3Y+(0.00), P3Z+(19.02)}, {P4X+(4.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-6.18), P5Y+(0.00), P5Z+(19.02)}, {P6X+(4.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(7.73), P1Y+(-0.00), P1Z+(23.78)}, {P2X+(-5.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(7.73), P3Y+(-0.00), P3Z+(23.78)}, {P4X+(-5.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(7.73), P5Y+(-0.00), P5Z+(23.78)}, {P6X+(-5.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(14.69), P1Y+(-0.00), P1Z+(20.23)}, {P2X+(-10.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(14.69), P3Y+(-0.00), P3Z+(20.23)}, {P4X+(-10.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(14.69), P5Y+(-0.00), P5Z+(20.23)}, {P6X+(-10.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(20.23), P1Y+(-0.00), P1Z+(14.69)}, {P2X+(-15.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(20.23), P3Y+(-0.00), P3Z+(14.69)}, {P4X+(-15.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(20.23), P5Y+(-0.00), P5Z+(14.69)}, {P6X+(-15.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(23.78), P1Y+(-0.00), P1Z+(7.73)}, {P2X+(-20.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(23.78), P3Y+(-0.00), P3Z+(7.73)}, {P4X+(-20.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(23.78), P5Y+(-0.00), P5Z+(7.73)}, {P6X+(-20.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(25.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-25.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(25.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-25.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(25.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-25.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(20.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-23.78), P2Y+(0.00), P2Z+(7.73)}, {P3X+(20.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-23.78), P4Y+(0.00), P4Z+(7.73)}, {P5X+(20.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-23.78), P6Y+(0.00), P6Z+(7.73)}}, + {{P1X+(15.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-20.23), P2Y+(0.00), P2Z+(14.69)}, {P3X+(15.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-20.23), P4Y+(0.00), P4Z+(14.69)}, {P5X+(15.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-20.23), P6Y+(0.00), P6Z+(14.69)}}, + {{P1X+(10.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-14.69), P2Y+(0.00), P2Z+(20.23)}, {P3X+(10.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-14.69), P4Y+(0.00), P4Z+(20.23)}, {P5X+(10.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-14.69), P6Y+(0.00), P6Z+(20.23)}}, + {{P1X+(5.00), P1Y+(-0.00), P1Z+(0.00)}, {P2X+(-7.73), P2Y+(0.00), P2Z+(23.78)}, {P3X+(5.00), P3Y+(-0.00), P3Z+(0.00)}, {P4X+(-7.73), P4Y+(0.00), P4Z+(23.78)}, {P5X+(5.00), P5Y+(-0.00), P5Z+(0.00)}, {P6X+(-7.73), P6Y+(0.00), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(-0.00), P6Z+(25.00)}}, + {{P1X+(-5.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(7.73), P2Y+(-0.00), P2Z+(23.78)}, {P3X+(-5.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(7.73), P4Y+(-0.00), P4Z+(23.78)}, {P5X+(-5.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(7.73), P6Y+(-0.00), P6Z+(23.78)}}, + {{P1X+(-10.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(14.69), P2Y+(-0.00), P2Z+(20.23)}, {P3X+(-10.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(14.69), P4Y+(-0.00), P4Z+(20.23)}, {P5X+(-10.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(14.69), P6Y+(-0.00), P6Z+(20.23)}}, + {{P1X+(-15.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(20.23), P2Y+(-0.00), P2Z+(14.69)}, {P3X+(-15.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(20.23), P4Y+(-0.00), P4Z+(14.69)}, {P5X+(-15.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(20.23), P6Y+(-0.00), P6Z+(14.69)}}, + {{P1X+(-20.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(23.78), P2Y+(-0.00), P2Z+(7.73)}, {P3X+(-20.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(23.78), P4Y+(-0.00), P4Z+(7.73)}, {P5X+(-20.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(23.78), P6Y+(-0.00), P6Z+(7.73)}}, + {{P1X+(-25.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(25.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-25.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(25.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-25.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(25.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-23.78), P1Y+(0.00), P1Z+(7.73)}, {P2X+(20.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-23.78), P3Y+(0.00), P3Z+(7.73)}, {P4X+(20.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-23.78), P5Y+(0.00), P5Z+(7.73)}, {P6X+(20.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-20.23), P1Y+(0.00), P1Z+(14.69)}, {P2X+(15.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-20.23), P3Y+(0.00), P3Z+(14.69)}, {P4X+(15.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-20.23), P5Y+(0.00), P5Z+(14.69)}, {P6X+(15.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-14.69), P1Y+(0.00), P1Z+(20.23)}, {P2X+(10.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-14.69), P3Y+(0.00), P3Z+(20.23)}, {P4X+(10.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-14.69), P5Y+(0.00), P5Z+(20.23)}, {P6X+(10.00), P6Y+(-0.00), P6Z+(0.00)}}, + {{P1X+(-7.73), P1Y+(0.00), P1Z+(23.78)}, {P2X+(5.00), P2Y+(-0.00), P2Z+(0.00)}, {P3X+(-7.73), P3Y+(0.00), P3Z+(23.78)}, {P4X+(5.00), P4Y+(-0.00), P4Z+(0.00)}, {P5X+(-7.73), P5Y+(0.00), P5Z+(23.78)}, {P6X+(5.00), P6Y+(-0.00), P6Z+(0.00)}}, }; const int shiftright_entries[] { 0,10 }; const MovementTable shiftright_table {shiftright_paths, 20, 20, shiftright_entries, 2 }; const Locations turnleft_paths[] { - {{P1X+(-0.00), P1Y+(0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}}, - {{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}}, - {{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}}, - {{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}}, - {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}}, - {{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}}, - {{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}}, - {{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}}, - {{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(20.00)}}, - {{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}}, - {{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}}, - {{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}}, - {{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}}, - {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}}, - {{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}}, - {{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}}, - {{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}}, - {{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}}, + {{P1X+(-0.00), P1Y+(0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}}, + {{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}}, + {{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}}, + {{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}}, + {{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}}, + {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}}, + {{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}}, + {{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}}, + {{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(0.00), P4Y+(-0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(-0.00), P6Y+(-0.00), P6Z+(25.00)}}, + {{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}}, + {{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}}, + {{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}}, + {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}}, + {{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}}, + {{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}}, + {{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}}, + {{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}}, + {{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), P6Z+(0.00)}}, }; const int turnleft_entries[] { 0,10 }; const MovementTable turnleft_table {turnleft_paths, 20, 20, turnleft_entries, 2 }; const Locations turnright_paths[] { - {{P1X+(0.00), P1Y+(-0.00), P1Z+(20.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(20.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(20.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, - {{P1X+(4.37), P1Y+(-4.37), P1Z+(19.02)}, {P2X+(0.00), P2Y+(4.00), P2Z+(0.00)}, {P3X+(-4.37), P3Y+(-4.37), P3Z+(19.02)}, {P4X+(2.83), P4Y+(-2.83), P4Z+(0.00)}, {P5X+(0.00), P5Y+(6.18), P5Z+(19.02)}, {P6X+(-2.83), P6Y+(-2.83), P6Z+(0.00)}}, - {{P1X+(8.31), P1Y+(-8.31), P1Z+(16.18)}, {P2X+(0.00), P2Y+(8.00), P2Z+(0.00)}, {P3X+(-8.31), P3Y+(-8.31), P3Z+(16.18)}, {P4X+(5.66), P4Y+(-5.66), P4Z+(0.00)}, {P5X+(0.00), P5Y+(11.76), P5Z+(16.18)}, {P6X+(-5.66), P6Y+(-5.66), P6Z+(0.00)}}, - {{P1X+(11.44), P1Y+(-11.44), P1Z+(11.76)}, {P2X+(0.00), P2Y+(12.00), P2Z+(0.00)}, {P3X+(-11.44), P3Y+(-11.44), P3Z+(11.76)}, {P4X+(8.49), P4Y+(-8.49), P4Z+(0.00)}, {P5X+(0.00), P5Y+(16.18), P5Z+(11.76)}, {P6X+(-8.49), P6Y+(-8.49), P6Z+(0.00)}}, - {{P1X+(13.45), P1Y+(-13.45), P1Z+(6.18)}, {P2X+(0.00), P2Y+(16.00), P2Z+(0.00)}, {P3X+(-13.45), P3Y+(-13.45), P3Z+(6.18)}, {P4X+(11.31), P4Y+(-11.31), P4Z+(0.00)}, {P5X+(0.00), P5Y+(19.02), P5Z+(6.18)}, {P6X+(-11.31), P6Y+(-11.31), P6Z+(0.00)}}, - {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}}, - {{P1X+(11.31), P1Y+(-11.31), P1Z+(0.00)}, {P2X+(0.00), P2Y+(19.02), P2Z+(6.18)}, {P3X+(-11.31), P3Y+(-11.31), P3Z+(0.00)}, {P4X+(13.45), P4Y+(-13.45), P4Z+(6.18)}, {P5X+(0.00), P5Y+(16.00), P5Z+(0.00)}, {P6X+(-13.45), P6Y+(-13.45), P6Z+(6.18)}}, - {{P1X+(8.49), P1Y+(-8.49), P1Z+(0.00)}, {P2X+(0.00), P2Y+(16.18), P2Z+(11.76)}, {P3X+(-8.49), P3Y+(-8.49), P3Z+(0.00)}, {P4X+(11.44), P4Y+(-11.44), P4Z+(11.76)}, {P5X+(0.00), P5Y+(12.00), P5Z+(0.00)}, {P6X+(-11.44), P6Y+(-11.44), P6Z+(11.76)}}, - {{P1X+(5.66), P1Y+(-5.66), P1Z+(0.00)}, {P2X+(0.00), P2Y+(11.76), P2Z+(16.18)}, {P3X+(-5.66), P3Y+(-5.66), P3Z+(0.00)}, {P4X+(8.31), P4Y+(-8.31), P4Z+(16.18)}, {P5X+(0.00), P5Y+(8.00), P5Z+(0.00)}, {P6X+(-8.31), P6Y+(-8.31), P6Z+(16.18)}}, - {{P1X+(2.83), P1Y+(-2.83), P1Z+(0.00)}, {P2X+(0.00), P2Y+(6.18), P2Z+(19.02)}, {P3X+(-2.83), P3Y+(-2.83), P3Z+(0.00)}, {P4X+(4.37), P4Y+(-4.37), P4Z+(19.02)}, {P5X+(0.00), P5Y+(4.00), P5Z+(0.00)}, {P6X+(-4.37), P6Y+(-4.37), P6Z+(19.02)}}, - {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(20.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(20.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(20.00)}}, - {{P1X+(-2.83), P1Y+(2.83), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-6.18), P2Z+(19.02)}, {P3X+(2.83), P3Y+(2.83), P3Z+(0.00)}, {P4X+(-4.37), P4Y+(4.37), P4Z+(19.02)}, {P5X+(-0.00), P5Y+(-4.00), P5Z+(0.00)}, {P6X+(4.37), P6Y+(4.37), P6Z+(19.02)}}, - {{P1X+(-5.66), P1Y+(5.66), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-11.76), P2Z+(16.18)}, {P3X+(5.66), P3Y+(5.66), P3Z+(0.00)}, {P4X+(-8.31), P4Y+(8.31), P4Z+(16.18)}, {P5X+(-0.00), P5Y+(-8.00), P5Z+(0.00)}, {P6X+(8.31), P6Y+(8.31), P6Z+(16.18)}}, - {{P1X+(-8.49), P1Y+(8.49), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-16.18), P2Z+(11.76)}, {P3X+(8.49), P3Y+(8.49), P3Z+(0.00)}, {P4X+(-11.44), P4Y+(11.44), P4Z+(11.76)}, {P5X+(-0.00), P5Y+(-12.00), P5Z+(0.00)}, {P6X+(11.44), P6Y+(11.44), P6Z+(11.76)}}, - {{P1X+(-11.31), P1Y+(11.31), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-19.02), P2Z+(6.18)}, {P3X+(11.31), P3Y+(11.31), P3Z+(0.00)}, {P4X+(-13.45), P4Y+(13.45), P4Z+(6.18)}, {P5X+(-0.00), P5Y+(-16.00), P5Z+(0.00)}, {P6X+(13.45), P6Y+(13.45), P6Z+(6.18)}}, - {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}}, - {{P1X+(-13.45), P1Y+(13.45), P1Z+(6.18)}, {P2X+(-0.00), P2Y+(-16.00), P2Z+(0.00)}, {P3X+(13.45), P3Y+(13.45), P3Z+(6.18)}, {P4X+(-11.31), P4Y+(11.31), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-19.02), P5Z+(6.18)}, {P6X+(11.31), P6Y+(11.31), P6Z+(0.00)}}, - {{P1X+(-11.44), P1Y+(11.44), P1Z+(11.76)}, {P2X+(-0.00), P2Y+(-12.00), P2Z+(0.00)}, {P3X+(11.44), P3Y+(11.44), P3Z+(11.76)}, {P4X+(-8.49), P4Y+(8.49), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-16.18), P5Z+(11.76)}, {P6X+(8.49), P6Y+(8.49), P6Z+(0.00)}}, - {{P1X+(-8.31), P1Y+(8.31), P1Z+(16.18)}, {P2X+(-0.00), P2Y+(-8.00), P2Z+(0.00)}, {P3X+(8.31), P3Y+(8.31), P3Z+(16.18)}, {P4X+(-5.66), P4Y+(5.66), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-11.76), P5Z+(16.18)}, {P6X+(5.66), P6Y+(5.66), P6Z+(0.00)}}, - {{P1X+(-4.37), P1Y+(4.37), P1Z+(19.02)}, {P2X+(-0.00), P2Y+(-4.00), P2Z+(0.00)}, {P3X+(4.37), P3Y+(4.37), P3Z+(19.02)}, {P4X+(-2.83), P4Y+(2.83), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-6.18), P5Z+(19.02)}, {P6X+(2.83), P6Y+(2.83), P6Z+(0.00)}}, + {{P1X+(0.00), P1Y+(-0.00), P1Z+(25.00)}, {P2X+(0.00), P2Y+(0.00), P2Z+(0.00)}, {P3X+(-0.00), P3Y+(-0.00), P3Z+(25.00)}, {P4X+(0.00), P4Y+(0.00), P4Z+(0.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(25.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(0.00)}}, + {{P1X+(5.46), P1Y+(-5.46), P1Z+(23.78)}, {P2X+(0.00), P2Y+(5.00), P2Z+(0.00)}, {P3X+(-5.46), P3Y+(-5.46), P3Z+(23.78)}, {P4X+(3.54), P4Y+(-3.54), P4Z+(0.00)}, {P5X+(0.00), P5Y+(7.73), P5Z+(23.78)}, {P6X+(-3.54), P6Y+(-3.54), P6Z+(0.00)}}, + {{P1X+(10.39), P1Y+(-10.39), P1Z+(20.23)}, {P2X+(0.00), P2Y+(10.00), P2Z+(0.00)}, {P3X+(-10.39), P3Y+(-10.39), P3Z+(20.23)}, {P4X+(7.07), P4Y+(-7.07), P4Z+(0.00)}, {P5X+(0.00), P5Y+(14.69), P5Z+(20.23)}, {P6X+(-7.07), P6Y+(-7.07), P6Z+(0.00)}}, + {{P1X+(14.30), P1Y+(-14.30), P1Z+(14.69)}, {P2X+(0.00), P2Y+(15.00), P2Z+(0.00)}, {P3X+(-14.30), P3Y+(-14.30), P3Z+(14.69)}, {P4X+(10.61), P4Y+(-10.61), P4Z+(0.00)}, {P5X+(0.00), P5Y+(20.23), P5Z+(14.69)}, {P6X+(-10.61), P6Y+(-10.61), P6Z+(0.00)}}, + {{P1X+(16.81), P1Y+(-16.81), P1Z+(7.73)}, {P2X+(0.00), P2Y+(20.00), P2Z+(0.00)}, {P3X+(-16.81), P3Y+(-16.81), P3Z+(7.73)}, {P4X+(14.14), P4Y+(-14.14), P4Z+(0.00)}, {P5X+(0.00), P5Y+(23.78), P5Z+(7.73)}, {P6X+(-14.14), P6Y+(-14.14), P6Z+(0.00)}}, + {{P1X+(17.68), P1Y+(-17.68), P1Z+(0.00)}, {P2X+(0.00), P2Y+(25.00), P2Z+(0.00)}, {P3X+(-17.68), P3Y+(-17.68), P3Z+(0.00)}, {P4X+(17.68), P4Y+(-17.68), P4Z+(0.00)}, {P5X+(0.00), P5Y+(25.00), P5Z+(0.00)}, {P6X+(-17.68), P6Y+(-17.68), P6Z+(0.00)}}, + {{P1X+(14.14), P1Y+(-14.14), P1Z+(0.00)}, {P2X+(0.00), P2Y+(23.78), P2Z+(7.73)}, {P3X+(-14.14), P3Y+(-14.14), P3Z+(0.00)}, {P4X+(16.81), P4Y+(-16.81), P4Z+(7.73)}, {P5X+(0.00), P5Y+(20.00), P5Z+(0.00)}, {P6X+(-16.81), P6Y+(-16.81), P6Z+(7.73)}}, + {{P1X+(10.61), P1Y+(-10.61), P1Z+(0.00)}, {P2X+(0.00), P2Y+(20.23), P2Z+(14.69)}, {P3X+(-10.61), P3Y+(-10.61), P3Z+(0.00)}, {P4X+(14.30), P4Y+(-14.30), P4Z+(14.69)}, {P5X+(0.00), P5Y+(15.00), P5Z+(0.00)}, {P6X+(-14.30), P6Y+(-14.30), P6Z+(14.69)}}, + {{P1X+(7.07), P1Y+(-7.07), P1Z+(0.00)}, {P2X+(0.00), P2Y+(14.69), P2Z+(20.23)}, {P3X+(-7.07), P3Y+(-7.07), P3Z+(0.00)}, {P4X+(10.39), P4Y+(-10.39), P4Z+(20.23)}, {P5X+(0.00), P5Y+(10.00), P5Z+(0.00)}, {P6X+(-10.39), P6Y+(-10.39), P6Z+(20.23)}}, + {{P1X+(3.54), P1Y+(-3.54), P1Z+(0.00)}, {P2X+(0.00), P2Y+(7.73), P2Z+(23.78)}, {P3X+(-3.54), P3Y+(-3.54), P3Z+(0.00)}, {P4X+(5.46), P4Y+(-5.46), P4Z+(23.78)}, {P5X+(0.00), P5Y+(5.00), P5Z+(0.00)}, {P6X+(-5.46), P6Y+(-5.46), P6Z+(23.78)}}, + {{P1X+(0.00), P1Y+(0.00), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-0.00), P2Z+(25.00)}, {P3X+(0.00), P3Y+(0.00), P3Z+(0.00)}, {P4X+(-0.00), P4Y+(0.00), P4Z+(25.00)}, {P5X+(0.00), P5Y+(0.00), P5Z+(0.00)}, {P6X+(0.00), P6Y+(0.00), P6Z+(25.00)}}, + {{P1X+(-3.54), P1Y+(3.54), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-7.73), P2Z+(23.78)}, {P3X+(3.54), P3Y+(3.54), P3Z+(0.00)}, {P4X+(-5.46), P4Y+(5.46), P4Z+(23.78)}, {P5X+(-0.00), P5Y+(-5.00), P5Z+(0.00)}, {P6X+(5.46), P6Y+(5.46), P6Z+(23.78)}}, + {{P1X+(-7.07), P1Y+(7.07), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-14.69), P2Z+(20.23)}, {P3X+(7.07), P3Y+(7.07), P3Z+(0.00)}, {P4X+(-10.39), P4Y+(10.39), P4Z+(20.23)}, {P5X+(-0.00), P5Y+(-10.00), P5Z+(0.00)}, {P6X+(10.39), P6Y+(10.39), P6Z+(20.23)}}, + {{P1X+(-10.61), P1Y+(10.61), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-20.23), P2Z+(14.69)}, {P3X+(10.61), P3Y+(10.61), P3Z+(0.00)}, {P4X+(-14.30), P4Y+(14.30), P4Z+(14.69)}, {P5X+(-0.00), P5Y+(-15.00), P5Z+(0.00)}, {P6X+(14.30), P6Y+(14.30), P6Z+(14.69)}}, + {{P1X+(-14.14), P1Y+(14.14), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-23.78), P2Z+(7.73)}, {P3X+(14.14), P3Y+(14.14), P3Z+(0.00)}, {P4X+(-16.81), P4Y+(16.81), P4Z+(7.73)}, {P5X+(-0.00), P5Y+(-20.00), P5Z+(0.00)}, {P6X+(16.81), P6Y+(16.81), P6Z+(7.73)}}, + {{P1X+(-17.68), P1Y+(17.68), P1Z+(0.00)}, {P2X+(-0.00), P2Y+(-25.00), P2Z+(0.00)}, {P3X+(17.68), P3Y+(17.68), P3Z+(0.00)}, {P4X+(-17.68), P4Y+(17.68), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-25.00), P5Z+(0.00)}, {P6X+(17.68), P6Y+(17.68), P6Z+(0.00)}}, + {{P1X+(-16.81), P1Y+(16.81), P1Z+(7.73)}, {P2X+(-0.00), P2Y+(-20.00), P2Z+(0.00)}, {P3X+(16.81), P3Y+(16.81), P3Z+(7.73)}, {P4X+(-14.14), P4Y+(14.14), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-23.78), P5Z+(7.73)}, {P6X+(14.14), P6Y+(14.14), P6Z+(0.00)}}, + {{P1X+(-14.30), P1Y+(14.30), P1Z+(14.69)}, {P2X+(-0.00), P2Y+(-15.00), P2Z+(0.00)}, {P3X+(14.30), P3Y+(14.30), P3Z+(14.69)}, {P4X+(-10.61), P4Y+(10.61), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-20.23), P5Z+(14.69)}, {P6X+(10.61), P6Y+(10.61), P6Z+(0.00)}}, + {{P1X+(-10.39), P1Y+(10.39), P1Z+(20.23)}, {P2X+(-0.00), P2Y+(-10.00), P2Z+(0.00)}, {P3X+(10.39), P3Y+(10.39), P3Z+(20.23)}, {P4X+(-7.07), P4Y+(7.07), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-14.69), P5Z+(20.23)}, {P6X+(7.07), P6Y+(7.07), P6Z+(0.00)}}, + {{P1X+(-5.46), P1Y+(5.46), P1Z+(23.78)}, {P2X+(-0.00), P2Y+(-5.00), P2Z+(0.00)}, {P3X+(5.46), P3Y+(5.46), P3Z+(23.78)}, {P4X+(-3.54), P4Y+(3.54), P4Z+(0.00)}, {P5X+(-0.00), P5Y+(-7.73), P5Z+(23.78)}, {P6X+(3.54), P6Y+(3.54), P6Z+(0.00)}}, }; const int turnright_entries[] { 0,10 }; const MovementTable turnright_table {turnright_paths, 20, 20, turnright_entries, 2 }; + +const Locations twist_paths[] { + {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 0.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 0.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 0.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 0.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 0.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}}, + {{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}}, + {{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, + {P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, + {P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, + {P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, + {P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, + {P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}}, + {{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, + {P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, + {P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, + {P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, + {P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, + {P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}}, + {{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, + {P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, + {P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, + {P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, + {P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, + {P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}}, + {{P1X*0.94 + P1Y*-0.33 + P1Z*0.07 + 0.00, P1X*0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*0.02 + P1Y*0.26 + P1Z*0.97 + 0.00}, + {P2X*0.94 + P2Y*-0.33 + P2Z*0.07 + 0.00, P2X*0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*0.02 + P2Y*0.26 + P2Z*0.97 + 0.00}, + {P3X*0.94 + P3Y*-0.33 + P3Z*0.07 + 0.00, P3X*0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*0.02 + P3Y*0.26 + P3Z*0.97 + 0.00}, + {P4X*0.94 + P4Y*-0.33 + P4Z*0.07 + 0.00, P4X*0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*0.02 + P4Y*0.26 + P4Z*0.97 + 0.00}, + {P5X*0.94 + P5Y*-0.33 + P5Z*0.07 + 0.00, P5X*0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*0.02 + P5Y*0.26 + P5Z*0.97 + 0.00}, + {P6X*0.94 + P6Y*-0.33 + P6Z*0.07 + 0.00, P6X*0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}}, + {{P1X*0.96 + P1Y*-0.27 + P1Z*0.05 + 0.00, P1X*0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, + {P2X*0.96 + P2Y*-0.27 + P2Z*0.05 + 0.00, P2X*0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, + {P3X*0.96 + P3Y*-0.27 + P3Z*0.05 + 0.00, P3X*0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, + {P4X*0.96 + P4Y*-0.27 + P4Z*0.05 + 0.00, P4X*0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, + {P5X*0.96 + P5Y*-0.27 + P5Z*0.05 + 0.00, P5X*0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, + {P6X*0.96 + P6Y*-0.27 + P6Z*0.05 + 0.00, P6X*0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}}, + {{P1X*0.98 + P1Y*-0.21 + P1Z*0.03 + 0.00, P1X*0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, + {P2X*0.98 + P2Y*-0.21 + P2Z*0.03 + 0.00, P2X*0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, + {P3X*0.98 + P3Y*-0.21 + P3Z*0.03 + 0.00, P3X*0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, + {P4X*0.98 + P4Y*-0.21 + P4Z*0.03 + 0.00, P4X*0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, + {P5X*0.98 + P5Y*-0.21 + P5Z*0.03 + 0.00, P5X*0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, + {P6X*0.98 + P6Y*-0.21 + P6Z*0.03 + 0.00, P6X*0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}}, + {{P1X*0.99 + P1Y*-0.14 + P1Z*0.01 + 0.00, P1X*0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, + {P2X*0.99 + P2Y*-0.14 + P2Z*0.01 + 0.00, P2X*0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, + {P3X*0.99 + P3Y*-0.14 + P3Z*0.01 + 0.00, P3X*0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, + {P4X*0.99 + P4Y*-0.14 + P4Z*0.01 + 0.00, P4X*0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, + {P5X*0.99 + P5Y*-0.14 + P5Z*0.01 + 0.00, P5X*0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, + {P6X*0.99 + P6Y*-0.14 + P6Z*0.01 + 0.00, P6X*0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}}, + {{P1X*1.00 + P1Y*-0.07 + P1Z*0.00 + 0.00, P1X*0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*-0.07 + P2Z*0.00 + 0.00, P2X*0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*-0.07 + P3Z*0.00 + 0.00, P3X*0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*-0.07 + P4Z*0.00 + 0.00, P4X*0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*-0.07 + P5Z*0.00 + 0.00, P5X*0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*-0.07 + P6Z*0.00 + 0.00, P6X*0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}}, + {{P1X*1.00 + P1Y*0.00 + P1Z*0.00 + 0.00, P1X*0.00 + P1Y*1.00 + P1Z*-0.05 + 0.00, P1X*0.00 + P1Y*0.05 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*0.00 + P2Z*0.00 + 0.00, P2X*0.00 + P2Y*1.00 + P2Z*-0.05 + 0.00, P2X*0.00 + P2Y*0.05 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*0.00 + P3Z*0.00 + 0.00, P3X*0.00 + P3Y*1.00 + P3Z*-0.05 + 0.00, P3X*0.00 + P3Y*0.05 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*0.00 + P4Z*0.00 + 0.00, P4X*0.00 + P4Y*1.00 + P4Z*-0.05 + 0.00, P4X*0.00 + P4Y*0.05 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*0.00 + P5Z*0.00 + 0.00, P5X*0.00 + P5Y*1.00 + P5Z*-0.05 + 0.00, P5X*0.00 + P5Y*0.05 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*0.00 + P6Z*0.00 + 0.00, P6X*0.00 + P6Y*1.00 + P6Z*-0.05 + 0.00, P6X*0.00 + P6Y*0.05 + P6Z*1.00 + 0.00}}, + {{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}}, + {{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, + {P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, + {P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, + {P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, + {P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, + {P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}}, + {{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, + {P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, + {P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, + {P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, + {P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, + {P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}}, + {{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, + {P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, + {P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, + {P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, + {P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, + {P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}}, + {{P1X*0.94 + P1Y*0.33 + P1Z*-0.07 + 0.00, P1X*-0.34 + P1Y*0.91 + P1Z*-0.25 + 0.00, P1X*-0.02 + P1Y*0.26 + P1Z*0.97 + 0.00}, + {P2X*0.94 + P2Y*0.33 + P2Z*-0.07 + 0.00, P2X*-0.34 + P2Y*0.91 + P2Z*-0.25 + 0.00, P2X*-0.02 + P2Y*0.26 + P2Z*0.97 + 0.00}, + {P3X*0.94 + P3Y*0.33 + P3Z*-0.07 + 0.00, P3X*-0.34 + P3Y*0.91 + P3Z*-0.25 + 0.00, P3X*-0.02 + P3Y*0.26 + P3Z*0.97 + 0.00}, + {P4X*0.94 + P4Y*0.33 + P4Z*-0.07 + 0.00, P4X*-0.34 + P4Y*0.91 + P4Z*-0.25 + 0.00, P4X*-0.02 + P4Y*0.26 + P4Z*0.97 + 0.00}, + {P5X*0.94 + P5Y*0.33 + P5Z*-0.07 + 0.00, P5X*-0.34 + P5Y*0.91 + P5Z*-0.25 + 0.00, P5X*-0.02 + P5Y*0.26 + P5Z*0.97 + 0.00}, + {P6X*0.94 + P6Y*0.33 + P6Z*-0.07 + 0.00, P6X*-0.34 + P6Y*0.91 + P6Z*-0.25 + 0.00, P6X*-0.02 + P6Y*0.26 + P6Z*0.97 + 0.00}}, + {{P1X*0.96 + P1Y*0.27 + P1Z*-0.05 + 0.00, P1X*-0.28 + P1Y*0.94 + P1Z*-0.21 + 0.00, P1X*-0.01 + P1Y*0.22 + P1Z*0.98 + 0.00}, + {P2X*0.96 + P2Y*0.27 + P2Z*-0.05 + 0.00, P2X*-0.28 + P2Y*0.94 + P2Z*-0.21 + 0.00, P2X*-0.01 + P2Y*0.22 + P2Z*0.98 + 0.00}, + {P3X*0.96 + P3Y*0.27 + P3Z*-0.05 + 0.00, P3X*-0.28 + P3Y*0.94 + P3Z*-0.21 + 0.00, P3X*-0.01 + P3Y*0.22 + P3Z*0.98 + 0.00}, + {P4X*0.96 + P4Y*0.27 + P4Z*-0.05 + 0.00, P4X*-0.28 + P4Y*0.94 + P4Z*-0.21 + 0.00, P4X*-0.01 + P4Y*0.22 + P4Z*0.98 + 0.00}, + {P5X*0.96 + P5Y*0.27 + P5Z*-0.05 + 0.00, P5X*-0.28 + P5Y*0.94 + P5Z*-0.21 + 0.00, P5X*-0.01 + P5Y*0.22 + P5Z*0.98 + 0.00}, + {P6X*0.96 + P6Y*0.27 + P6Z*-0.05 + 0.00, P6X*-0.28 + P6Y*0.94 + P6Z*-0.21 + 0.00, P6X*-0.01 + P6Y*0.22 + P6Z*0.98 + 0.00}}, + {{P1X*0.98 + P1Y*0.21 + P1Z*-0.03 + 0.00, P1X*-0.21 + P1Y*0.96 + P1Z*-0.17 + 0.00, P1X*-0.01 + P1Y*0.18 + P1Z*0.98 + 0.00}, + {P2X*0.98 + P2Y*0.21 + P2Z*-0.03 + 0.00, P2X*-0.21 + P2Y*0.96 + P2Z*-0.17 + 0.00, P2X*-0.01 + P2Y*0.18 + P2Z*0.98 + 0.00}, + {P3X*0.98 + P3Y*0.21 + P3Z*-0.03 + 0.00, P3X*-0.21 + P3Y*0.96 + P3Z*-0.17 + 0.00, P3X*-0.01 + P3Y*0.18 + P3Z*0.98 + 0.00}, + {P4X*0.98 + P4Y*0.21 + P4Z*-0.03 + 0.00, P4X*-0.21 + P4Y*0.96 + P4Z*-0.17 + 0.00, P4X*-0.01 + P4Y*0.18 + P4Z*0.98 + 0.00}, + {P5X*0.98 + P5Y*0.21 + P5Z*-0.03 + 0.00, P5X*-0.21 + P5Y*0.96 + P5Z*-0.17 + 0.00, P5X*-0.01 + P5Y*0.18 + P5Z*0.98 + 0.00}, + {P6X*0.98 + P6Y*0.21 + P6Z*-0.03 + 0.00, P6X*-0.21 + P6Y*0.96 + P6Z*-0.17 + 0.00, P6X*-0.01 + P6Y*0.18 + P6Z*0.98 + 0.00}}, + {{P1X*0.99 + P1Y*0.14 + P1Z*-0.01 + 0.00, P1X*-0.14 + P1Y*0.98 + P1Z*-0.13 + 0.00, P1X*-0.01 + P1Y*0.14 + P1Z*0.99 + 0.00}, + {P2X*0.99 + P2Y*0.14 + P2Z*-0.01 + 0.00, P2X*-0.14 + P2Y*0.98 + P2Z*-0.13 + 0.00, P2X*-0.01 + P2Y*0.14 + P2Z*0.99 + 0.00}, + {P3X*0.99 + P3Y*0.14 + P3Z*-0.01 + 0.00, P3X*-0.14 + P3Y*0.98 + P3Z*-0.13 + 0.00, P3X*-0.01 + P3Y*0.14 + P3Z*0.99 + 0.00}, + {P4X*0.99 + P4Y*0.14 + P4Z*-0.01 + 0.00, P4X*-0.14 + P4Y*0.98 + P4Z*-0.13 + 0.00, P4X*-0.01 + P4Y*0.14 + P4Z*0.99 + 0.00}, + {P5X*0.99 + P5Y*0.14 + P5Z*-0.01 + 0.00, P5X*-0.14 + P5Y*0.98 + P5Z*-0.13 + 0.00, P5X*-0.01 + P5Y*0.14 + P5Z*0.99 + 0.00}, + {P6X*0.99 + P6Y*0.14 + P6Z*-0.01 + 0.00, P6X*-0.14 + P6Y*0.98 + P6Z*-0.13 + 0.00, P6X*-0.01 + P6Y*0.14 + P6Z*0.99 + 0.00}}, + {{P1X*1.00 + P1Y*0.07 + P1Z*-0.00 + 0.00, P1X*-0.07 + P1Y*0.99 + P1Z*-0.09 + 0.00, P1X*-0.00 + P1Y*0.09 + P1Z*1.00 + 0.00}, + {P2X*1.00 + P2Y*0.07 + P2Z*-0.00 + 0.00, P2X*-0.07 + P2Y*0.99 + P2Z*-0.09 + 0.00, P2X*-0.00 + P2Y*0.09 + P2Z*1.00 + 0.00}, + {P3X*1.00 + P3Y*0.07 + P3Z*-0.00 + 0.00, P3X*-0.07 + P3Y*0.99 + P3Z*-0.09 + 0.00, P3X*-0.00 + P3Y*0.09 + P3Z*1.00 + 0.00}, + {P4X*1.00 + P4Y*0.07 + P4Z*-0.00 + 0.00, P4X*-0.07 + P4Y*0.99 + P4Z*-0.09 + 0.00, P4X*-0.00 + P4Y*0.09 + P4Z*1.00 + 0.00}, + {P5X*1.00 + P5Y*0.07 + P5Z*-0.00 + 0.00, P5X*-0.07 + P5Y*0.99 + P5Z*-0.09 + 0.00, P5X*-0.00 + P5Y*0.09 + P5Z*1.00 + 0.00}, + {P6X*1.00 + P6Y*0.07 + P6Z*-0.00 + 0.00, P6X*-0.07 + P6Y*0.99 + P6Z*-0.09 + 0.00, P6X*-0.00 + P6Y*0.09 + P6Z*1.00 + 0.00}}, +}; +const int twist_entries[] { 0,10 }; +const MovementTable twist_table {twist_paths, 20, 50, twist_entries, 2 }; } const MovementTable& backwardTable() { return backward_table; } +const MovementTable& climbTable() { + return climb_table; +} const MovementTable& forwardTable() { return forward_table; } +const MovementTable& forwardfastTable() { + return forwardfast_table; +} const MovementTable& rotatexTable() { return rotatex_table; } @@ -556,3 +737,6 @@ const MovementTable& turnleftTable() { const MovementTable& turnrightTable() { return turnright_table; } +const MovementTable& twistTable() { + return twist_table; +} diff --git a/software/hexapod7697/src/normal_mode.cpp b/software/hexapod7697/src/normal_mode.cpp index 2e5026e..d1b35c4 100644 --- a/software/hexapod7697/src/normal_mode.cpp +++ b/software/hexapod7697/src/normal_mode.cpp @@ -12,15 +12,19 @@ #define REACT_DELAY hexapod::config::movementInterval -static Button buttonForward(hexapod::MOVEMENT_FORWARD, "Forward", 1, 1, 1, 1, RC_BLUE); -static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 3, 1, 1, RC_BLUE); -static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 2, 1, 1, RC_BLUE); -static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 2, 1, 1, RC_BLUE); -static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 3, 1, 1, RC_YELLOW); -static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 3, 1, 1, RC_YELLOW); +static Button buttonForward(hexapod::MOVEMENT_FORWARD, "Forward", 1, 3, 1, 1, RC_BLUE); +static Button buttonRun(hexapod::MOVEMENT_FORWARDFAST, "Run", 1, 2, 1, 1, RC_ORANGE); +static Button buttonBackward(hexapod::MOVEMENT_BACKWARD, "Backward", 1, 4, 1, 1, RC_BLUE); +static Button buttonTL(hexapod::MOVEMENT_TURNLEFT, "TurnLeft", 0, 3, 1, 1, RC_GREEN); +static Button buttonTR(hexapod::MOVEMENT_TURNRIGHT, "TurnRight", 2, 3, 1, 1, RC_GREEN); +static Button buttonSL(hexapod::MOVEMENT_SHIFTLEFT, "ShiftLeft", 0, 4, 1, 1, RC_BLUE); +static Button buttonSR(hexapod::MOVEMENT_SHIFTRIGHT, "ShiftRight", 2, 4, 1, 1, RC_BLUE); +static Button buttonClimb(hexapod::MOVEMENT_CLIMB, "Climb", 2, 2, 1, 1, RC_ORANGE); + static Button buttonRotateX(hexapod::MOVEMENT_ROTATEX, "RotateX", 0, 0, 1, 1, RC_YELLOW); static Button buttonRotateY(hexapod::MOVEMENT_ROTATEY, "RotateY", 1, 0, 1, 1, RC_YELLOW); static Button buttonRotateZ(hexapod::MOVEMENT_ROTATEZ, "RotateZ", 2, 0, 1, 1, RC_YELLOW); +static Button buttonTwist(hexapod::MOVEMENT_TWIST, "Twist", 0, 1, 1, 1, RC_YELLOW); static ButtonGroup btnGroup; @@ -37,13 +41,17 @@ void normal_setup(void) { &buttonForward, &buttonBackward, &buttonTL, &buttonTR, &buttonSL, &buttonSR, + &buttonRun, &buttonClimb, &buttonRotateX, &buttonRotateY, &buttonRotateZ, + &buttonTwist, }); btnGroup.addControls({ &buttonForward, &buttonBackward, &buttonTL, &buttonTR, &buttonSL, &buttonSR, + &buttonRun, &buttonClimb, &buttonRotateX, &buttonRotateY, &buttonRotateZ, + &buttonTwist, }); LRemote.begin(); diff --git a/software/pathTool/src/config.py b/software/pathTool/src/config.py new file mode 100644 index 0000000..61dc4c6 --- /dev/null +++ b/software/pathTool/src/config.py @@ -0,0 +1,48 @@ +kLegMountLeftRightX = 29.87 +kLegMountOtherX = 22.41 +kLegMountOtherY = 55.41 + +kLegRootToJoint1 = 20.75 +kLegJoint1ToJoint2 = 28.0 +kLegJoint2ToJoint3 = 42.6 +kLegJoint3ToTip = 89.07 + +SIN30 = 0.5 +COS30 = 0.866 +SIN45 = 0.7071 +COS45 = 0.7071 +SIN15 = 0.2588 +COS15 = 0.9659 + +STANDBY_Z = (kLegJoint3ToTip*COS15-kLegJoint2ToJoint3*SIN30) +LEFTRIGHT_X = (kLegMountLeftRightX+kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15) +OTHER_X = (kLegMountOtherX + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*COS45) +OTHER_Y = (kLegMountOtherY + (kLegRootToJoint1+kLegJoint1ToJoint2+(kLegJoint2ToJoint3*COS30)+kLegJoint3ToTip*SIN15)*SIN45) + +defaultPosition = ( + (OTHER_X, OTHER_Y, -STANDBY_Z), + (LEFTRIGHT_X, 0, -STANDBY_Z), + (OTHER_X, -OTHER_Y, -STANDBY_Z), + (-OTHER_X, -OTHER_Y, -STANDBY_Z), + (-LEFTRIGHT_X, 0, -STANDBY_Z), + (-OTHER_X, OTHER_Y, -STANDBY_Z), +) + +mountPosition = ( + (kLegMountOtherX, kLegMountOtherY, 0), + (kLegMountLeftRightX, 0, 0), + (kLegMountOtherX, -kLegMountOtherY, 0), + (-kLegMountOtherX, -kLegMountOtherY, 0), + (-kLegMountLeftRightX, 0, 0), + (-kLegMountOtherX, kLegMountOtherY, 0), +) + +defaultAngle = ( + -45, 0, 45, 135, 180, 225 +) + +angleLimitation = ( + (-45, 45), + (-45, 75), + (-60, 60), +) diff --git a/software/pathTool/src/kinematics.py b/software/pathTool/src/kinematics.py new file mode 100644 index 0000000..583e4c3 --- /dev/null +++ b/software/pathTool/src/kinematics.py @@ -0,0 +1,25 @@ +import math + +import config + +pi = math.acos(-1) + +def ik(to): + angles = [] + x = to[0] - config.kLegRootToJoint1 + y = to[1] + + angles.append(math.atan2(y, x) * 180 / pi) + + x = math.sqrt(x*x + y*y) - config.kLegJoint1ToJoint2 + y = to[2] + ar = math.atan2(y, x) + lr2 = x*x + y*y + lr = math.sqrt(lr2) + a1 = math.acos((lr2 + config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 - config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint2ToJoint3*lr)) + a2 = math.acos((lr2 - config.kLegJoint2ToJoint3*config.kLegJoint2ToJoint3 + config.kLegJoint3ToTip*config.kLegJoint3ToTip)/(2*config.kLegJoint3ToTip*lr)) + + angles.append((ar + a1) * 180 / pi) + angles.append(90 - ((a1 + a2) * 180 / pi)) + + return angles \ No newline at end of file diff --git a/software/pathTool/src/main.py b/software/pathTool/src/main.py index c4e245e..e3bd859 100644 --- a/software/pathTool/src/main.py +++ b/software/pathTool/src/main.py @@ -3,6 +3,10 @@ import logging import os import sys +import config +import kinematics +from path.lib import point_rotate_z, matrix_mul + def collectPath(sub_folder): scripts = {} for script_name in [f[:-3] for f in sorted(os.listdir(sub_folder)) if f.endswith('.py') and os.path.isfile(os.path.join(sub_folder, f))]: @@ -18,6 +22,55 @@ def show_detail(path, result): for i, p in enumerate(result): print("{:2d} {:5.2f}, {:5.2f}, {:5.2f}".format(i, p[0], p[1], p[2])) +def verify_points(pt): + angles = kinematics.ik(pt) + + ok = True + failed = [] + for i, angle in enumerate(angles): + if angle < config.angleLimitation[i][0] or angle > config.angleLimitation[i][1]: + ok = False + failed.append((i, angle)) + + return ok, failed + +def verify_path(path, params): + data, mode, _, _ = params + print("Verifying {}...".format(path)) + + all_ok = True + if mode == "shift": + # data: float[6][N][3] + assert(len(data) == 6) + + for i in range(len(data[0])): + for j in range(6): + pt = [config.defaultPosition[j][k] - config.mountPosition[j][k] + data[j][i][k] for k in range(3)] + pt = point_rotate_z(pt, config.defaultAngle[j]) + ok, failed = verify_points(pt) + + if not ok: + print("{}, {} failed: {}".format(i, j, failed)) + all_ok = False + + elif mode == "matrix": + # data: np.matrix[N] + for i in range(len(data)): + for j in range(6): + pt = matrix_mul(data[i], config.defaultPosition[j]) + for k in range(3): + pt[k] -= config.mountPosition[j][k] + pt = point_rotate_z(pt, config.defaultAngle[j]) + + ok, failed = verify_points(pt) + + if not ok: + print("{}, {} failed: {}".format(i, j, failed)) + all_ok = False + + return all_ok + + def generate_c_body(path, params): data, mode, dur, entries = params result = "\nconst Locations {}_paths[] {{\n".format(path) @@ -77,19 +130,24 @@ if __name__ == '__main__': # generate all paths results = {path: generator() for path, generator in paths.items()} - # output results - with open(args.out_path, "w") as f: - print("//", file=f) - print("// This file is generated, dont directly modify content...", file=f) - print("//", file=f) - print("namespace {", file=f) - for path, data in results.items(): - print(generate_c_body(path, data), file=f) - print("}\n", file=f) - for path in results: - print(generate_c_def(path), file=f) - - print("Result written to {}".format(args.out_path)) - + # verify all path is within safe angles + + verified = [1 for path, data in results.items() if not verify_path(path, data)] + if len(verified) > 0: + print("There were errors, exit...") + else: + # output results + with open(args.out_path, "w") as f: + print("//", file=f) + print("// This file is generated, dont directly modify content...", file=f) + print("//", file=f) + print("namespace {", file=f) + for path, data in results.items(): + print(generate_c_body(path, data), file=f) + print("}\n", file=f) + for path in results: + print(generate_c_def(path), file=f) + + print("Result written to {}".format(args.out_path)) diff --git a/software/pathTool/src/path/__init__.py b/software/pathTool/src/path/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/pathTool/src/path/backward.py b/software/pathTool/src/path/backward.py index 12302e5..7b717ce 100644 --- a/software/pathTool/src/path/backward.py +++ b/software/pathTool/src/path/backward.py @@ -1,7 +1,9 @@ from collections import deque from lib import semicircle_generator -from forward import g_steps, g_radius + +g_steps = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/climb.py b/software/pathTool/src/path/climb.py new file mode 100644 index 0000000..c7e2f09 --- /dev/null +++ b/software/pathTool/src/path/climb.py @@ -0,0 +1,27 @@ + +from collections import deque + +from lib import semicircle2_generator + +g_steps = 20 +y_radius = 20 +z_radius = 80 +x_radius = 30 + +z_shift = -30 + +def path_generator(): + assert (g_steps % 4) == 0 + halfsteps = int(g_steps/2) + + rpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, x_radius)] + lpath = [(x, y, z + z_shift) for x, y, z in semicircle2_generator(g_steps, y_radius, z_radius, -x_radius)] + + mir_rpath = deque(rpath) + mir_rpath.rotate(halfsteps) + + mir_lpath = deque(lpath) + mir_lpath.rotate(halfsteps) + + return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 30, (0, halfsteps) + \ No newline at end of file diff --git a/software/pathTool/src/path/forward.py b/software/pathTool/src/path/forward.py index efb40b4..afb44a4 100644 --- a/software/pathTool/src/path/forward.py +++ b/software/pathTool/src/path/forward.py @@ -4,7 +4,7 @@ from collections import deque from lib import semicircle_generator g_steps = 20 -g_radius = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/forwardfast.py b/software/pathTool/src/path/forwardfast.py new file mode 100644 index 0000000..ce4259f --- /dev/null +++ b/software/pathTool/src/path/forwardfast.py @@ -0,0 +1,25 @@ + +from collections import deque + +from lib import semicircle2_generator + +g_steps = 20 +y_radius = 50 +z_radius = 30 +x_radius = 10 + +def path_generator(): + assert (g_steps % 4) == 0 + halfsteps = int(g_steps/2) + + rpath = semicircle2_generator(g_steps, y_radius, z_radius, x_radius) + lpath = semicircle2_generator(g_steps, y_radius, z_radius, -x_radius) + + mir_rpath = deque(rpath) + mir_rpath.rotate(halfsteps) + + mir_lpath = deque(lpath) + mir_lpath.rotate(halfsteps) + + return [rpath, mir_rpath, rpath, mir_lpath, lpath, mir_lpath, ], "shift", 20, (0, halfsteps) + \ No newline at end of file diff --git a/software/pathTool/src/path/lib.py b/software/pathTool/src/path/lib.py index c6824a9..6d22749 100644 --- a/software/pathTool/src/path/lib.py +++ b/software/pathTool/src/path/lib.py @@ -32,6 +32,35 @@ def semicircle_generator(radius, steps, reverse=False): return result +def semicircle2_generator(steps, y_radius, z_radius, x_radius, reverse=False): + assert (steps % 4) == 0 + halfsteps = int(steps/2) + + step_angle = pi / halfsteps + + result = [] + + # first half, move backward (only y change) + for i in range(halfsteps): + result.append((0, y_radius - i*y_radius*2/(halfsteps), 0)) + + # second half, move forward in semicircle shape (y, z change) + for i in range(halfsteps): + angle = pi - step_angle*i + y = y_radius * math.cos(angle) + z = z_radius * math.sin(angle) + x = x_radius * math.sin(angle) + result.append((x, y, z)) + + result = deque(result) + result.rotate(int(steps/4)) + + if reverse: + result = deque(reversed(result)) + result.rotate(1) + + return result + def get_rotate_x_matrix(angle): angle = angle * pi / 180 return np.matrix([ @@ -59,6 +88,10 @@ def get_rotate_z_matrix(angle): [0, 0, 0, 1], ]) +def matrix_mul(m, pt): + ptx = list(pt) + [1] + return list((m * np.matrix(ptx).T).T.flat)[:-1] + def point_rotate_x(pt, angle): ptx = list(pt) + [1] return list((get_rotate_x_matrix(angle) * np.matrix(ptx).T).T.flat)[:-1] diff --git a/software/pathTool/src/path/shiftleft.py b/software/pathTool/src/path/shiftleft.py index a24766e..fff60e8 100644 --- a/software/pathTool/src/path/shiftleft.py +++ b/software/pathTool/src/path/shiftleft.py @@ -5,7 +5,7 @@ from lib import semicircle_generator from lib import path_rotate_z g_steps = 20 -g_radius = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/shiftright.py b/software/pathTool/src/path/shiftright.py index 81a1bc8..d723957 100644 --- a/software/pathTool/src/path/shiftright.py +++ b/software/pathTool/src/path/shiftright.py @@ -5,7 +5,7 @@ from lib import semicircle_generator from lib import path_rotate_z g_steps = 20 -g_radius = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/turnleft.py b/software/pathTool/src/path/turnleft.py index 11016fb..fb4e21b 100644 --- a/software/pathTool/src/path/turnleft.py +++ b/software/pathTool/src/path/turnleft.py @@ -5,7 +5,7 @@ from lib import semicircle_generator from lib import path_rotate_z g_steps = 20 -g_radius = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/turnright.py b/software/pathTool/src/path/turnright.py index c3e612e..af48a36 100644 --- a/software/pathTool/src/path/turnright.py +++ b/software/pathTool/src/path/turnright.py @@ -5,7 +5,7 @@ from lib import semicircle_generator from lib import path_rotate_z g_steps = 20 -g_radius = 20 +g_radius = 25 def path_generator(): assert (g_steps % 4) == 0 diff --git a/software/pathTool/src/path/twist.py b/software/pathTool/src/path/twist.py new file mode 100644 index 0000000..d567fa5 --- /dev/null +++ b/software/pathTool/src/path/twist.py @@ -0,0 +1,35 @@ + +from collections import deque +import math + +from lib import get_rotate_x_matrix, get_rotate_z_matrix + +g_steps = 20 + +raise_angle = 3 +twist_x_angle = 20 +twise_y_angle = 12 + +def path_generator(): + assert (g_steps % 4) == 0 + + result = [] + + quarter = int(g_steps / 4) + step_x_angle = twist_x_angle / quarter + step_y_angle = twise_y_angle / quarter + + m = get_rotate_x_matrix(raise_angle) + for i in range(quarter): + result.append(m * get_rotate_z_matrix(i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle)) + + for i in range(quarter): + result.append(m * get_rotate_z_matrix((quarter-i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle)) + + for i in range(quarter): + result.append(m * get_rotate_z_matrix(-i*step_x_angle) * get_rotate_x_matrix(i*step_y_angle)) + + for i in range(quarter): + result.append(m * get_rotate_z_matrix((-quarter+i)*step_x_angle) * get_rotate_x_matrix((quarter-i)*step_y_angle)) + + return result, "matrix", 50, [0, 10] \ No newline at end of file