From 6b9cdb440bd21ef3a1e141c61d253e96489b311b Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 9 Sep 2016 16:33:13 -0700
Subject: [PATCH] update teleport.js to teleport HMD before avatar

(in theory, not tested)
---
 scripts/system/controllers/teleport.js | 40 +++++++++++++++++++++-----
 1 file changed, 33 insertions(+), 7 deletions(-)

diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js
index b4a8eefcd2..c5a7a5e81a 100644
--- a/scripts/system/controllers/teleport.js
+++ b/scripts/system/controllers/teleport.js
@@ -44,6 +44,7 @@ var COLORS_TELEPORT_TOO_CLOSE = {
 var TELEPORT_CANCEL_RANGE = 1;
 var USE_COOL_IN = true;
 var COOL_IN_DURATION = 500;
+var MAX_HMD_AVATAR_SEPARATION = 10.0f;
 
 function ThumbPad(hand) {
     this.hand = hand;
@@ -88,6 +89,8 @@ function Teleporter() {
     this.updateConnected = null;
     this.smoothArrivalInterval = null;
     this.teleportHand = null;
+    this.distance = 0.0;
+    this.teleportMode = "HMDAndAvatarTogether";
     this.tooClose = false;
     this.inCoolIn = false;
 
@@ -459,7 +462,8 @@ function Teleporter() {
             z: intersection.intersection.z
         };
 
-        this.tooClose = isTooCloseToTeleport(position);
+        this.distance = Vec3.distance(MyAvatar.position, position);
+        this.tooClose = this.distance <= TELEPORT_CANCEL_RANGE;
         var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0);
 
         Overlays.editOverlay(this.targetOverlay, {
@@ -480,7 +484,8 @@ function Teleporter() {
             z: intersection.intersection.z
         };
 
-        this.tooClose = isTooCloseToTeleport(position);
+        this.distance = Vec3.distance(MyAvatar.position, position);
+        this.tooClose = this.distance <= TELEPORT_CANCEL_RANGE;
         var towardUs = Quat.fromPitchYawRollDegrees(0, euler.y, 0);
 
         Overlays.editOverlay(this.cancelOverlay, {
@@ -509,6 +514,15 @@ function Teleporter() {
             var offset = getAvatarFootOffset();
             this.intersection.intersection.y += offset;
             this.exitTeleportMode();
+            if (MyAvatar.hmdLeanRecenterEnabled) {
+                if (this.distance > MAX_HMD_AVATAR_SEPARATION) {
+                    this.teleportMode = "HMDAndAvatarTogether";
+                } else {
+                    this.teleportMode = "HMDFirstAvatarWillFollow";
+                }
+            } else {
+                this.teleportMode = "AvatarOnly";
+            }
             this.smoothArrival();
         }
     };
@@ -538,6 +552,22 @@ function Teleporter() {
         return arrivalPoints;
     };
 
+    this.teleportTo = function(landingPoint) {
+        if (this.teleportMode === "AvatarOnly") {
+            MyAvatar.position = landingPoint;
+        } else if (this.teleportMode === "HMDAndAvatarTogether") {
+            var leanEnabled = MyAvatar.hmdLeanRecenterEnabled;
+            MyAvatar.setHMDLeanRecenterEnabled(false);
+            MyAvatar.position = landingPoint;
+            HMD.snapToAvatar();
+            MyAvatar.hmdLeanRecenterEnabled = leanEnabled;
+        } else if (this.teleportMode === "HMDFirstAvatarWillFollow") {
+            var eyeOffset = Vec3.subtract(MyAvatar.getEyePosition(), MyAvatar.position);
+            landingPoint = Vec3.sum(landingPoint, eyeOffset);
+            HMD.setPosition(landingPoint);
+        }
+    }
+
     this.smoothArrival = function() {
 
         _this.arrivalPoints = _this.getArrivalPoints(MyAvatar.position, _this.intersection.intersection);
@@ -548,7 +578,7 @@ function Teleporter() {
                 return;
             }
             var landingPoint = _this.arrivalPoints.shift();
-            MyAvatar.position = landingPoint;
+            _this.teleportTo(landingPoint);
 
             if (_this.arrivalPoints.length === 1 || _this.arrivalPoints.length === 0) {
                 _this.deleteTargetOverlay();
@@ -627,10 +657,6 @@ function isMoving() {
     }
 };
 
-function isTooCloseToTeleport(position) {
-    return Vec3.distance(MyAvatar.position, position) <= TELEPORT_CANCEL_RANGE;
-};
-
 function registerMappings() {
     mappingName = 'Hifi-Teleporter-Dev-' + Math.random();
     teleportMapping = Controller.newMapping(mappingName);