From ffdb10681e28c2f78ae70773215832bbfa198f4d Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 22 Aug 2017 11:07:04 -0700
Subject: [PATCH] Fix for laser rendering and positioning with sensor scale

---
 interface/src/Application.cpp     | 24 +++++++++++++++---------
 interface/src/avatar/MyAvatar.cpp |  9 +++++++--
 interface/src/avatar/MyAvatar.h   |  2 +-
 3 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 560cb8c00d..93aa5791d0 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2475,6 +2475,13 @@ void Application::paintGL() {
         finalFramebuffer = framebufferCache->getFramebuffer();
     }
 
+    auto hmdInterface = DependencyManager::get<HMDScriptingInterface>();
+    float ipdScale = hmdInterface->getIPDScale();
+
+    // scale IPD by height ratio, to make the world seem larger or smaller accordingly.
+    float heightRatio = getMyAvatar()->getEyeHeight() / getMyAvatar()->getUserEyeHeight();
+    ipdScale *= heightRatio;
+
     mat4 eyeProjections[2];
     {
         PROFILE_RANGE(render, "/mainRender");
@@ -2499,13 +2506,6 @@ void Application::paintGL() {
             mat4 eyeOffsets[2];
             mat4 eyeProjections[2];
 
-            auto hmdInterface = DependencyManager::get<HMDScriptingInterface>();
-            float IPDScale = hmdInterface->getIPDScale();
-
-            // scale IPD by height ratio, to make the world seem larger or smaller accordingly.
-            float heightRatio = getMyAvatar()->getEyeHeight() / getMyAvatar()->getUserEyeHeight();
-            IPDScale *= heightRatio;
-
             // adjust near clip plane by heightRatio
             auto baseProjection = glm::perspective(renderArgs.getViewFrustum().getFieldOfView(),
                                                    renderArgs.getViewFrustum().getAspectRatio(),
@@ -2524,7 +2524,7 @@ void Application::paintGL() {
                 // Grab the translation
                 vec3 eyeOffset = glm::vec3(eyeToHead[3]);
                 // Apply IPD scaling
-                mat4 eyeOffsetTransform = glm::translate(mat4(), eyeOffset * -1.0f * IPDScale);
+                mat4 eyeOffsetTransform = glm::translate(mat4(), eyeOffset * -1.0f * ipdScale);
                 eyeOffsets[eye] = eyeOffsetTransform;
                 eyeProjections[eye] = displayPlugin->getEyeProjection(eye, baseProjection);
             });
@@ -2544,8 +2544,14 @@ void Application::paintGL() {
         PerformanceTimer perfTimer("postComposite");
         renderArgs._batch = &postCompositeBatch;
         renderArgs._batch->setViewportTransform(ivec4(0, 0, finalFramebufferSize.width(), finalFramebufferSize.height()));
-        renderArgs._batch->setViewTransform(renderArgs.getViewFrustum().getView());
         for_each_eye([&](Eye eye) {
+
+            // apply eye offset and IPD scale to the view matrix
+            mat4 eyeToHead = displayPlugin->getEyeToHeadTransform(eye);
+            vec3 eyeOffset = glm::vec3(eyeToHead[3]);
+            mat4 eyeOffsetTransform = glm::translate(mat4(), eyeOffset * -1.0f * ipdScale);
+            renderArgs._batch->setViewTransform(renderArgs.getViewFrustum().getView() * eyeOffsetTransform);
+
             renderArgs._batch->setProjectionTransform(eyeProjections[eye]);
             _overlays.render3DHUDOverlays(&renderArgs);
         });
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index d13fa1cc24..4c79554a69 100755
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -667,8 +667,8 @@ void MyAvatar::updateSensorToWorldMatrix() {
 
     // update the sensor mat so that the body position will end up in the desired
     // position when driven from the head.
-    float heightRatio = getEyeHeight() / getUserEyeHeight();
-    glm::mat4 desiredMat = createMatFromScaleQuatAndPos(glm::vec3(heightRatio), getOrientation(), getPosition());
+    float sensorToWorldScale = getEyeHeight() / getUserEyeHeight();
+    glm::mat4 desiredMat = createMatFromScaleQuatAndPos(glm::vec3(sensorToWorldScale), getOrientation(), getPosition());
     _sensorToWorldMatrix = desiredMat * glm::inverse(_bodySensorMatrix);
 
     lateUpdatePalms();
@@ -679,6 +679,7 @@ void MyAvatar::updateSensorToWorldMatrix() {
     }
 
     _sensorToWorldMatrixCache.set(_sensorToWorldMatrix);
+    _sensorToWorldScaleCache.set(sensorToWorldScale);
 
     updateJointFromController(controller::Action::LEFT_HAND, _controllerLeftHandMatrixCache);
     updateJointFromController(controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache);
@@ -2872,6 +2873,10 @@ glm::mat4 MyAvatar::computeCameraRelativeHandControllerMatrix(const glm::mat4& c
     // move the camera into sensor space.
     glm::mat4 cameraSensorMatrix = glm::inverse(getSensorToWorldMatrix()) * cameraWorldMatrix;
 
+    // cancel out scale
+    glm::vec3 scale = extractScale(cameraSensorMatrix);
+    cameraSensorMatrix = glm::scale(cameraSensorMatrix, 1.0f / scale);
+
     // measure the offset from the hmd and the camera, in sensor space
     glm::mat4 delta = cameraSensorMatrix * glm::inverse(getHMDSensorMatrix());
 
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 3d335d0a19..ecd7be2b7b 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -708,7 +708,7 @@ private:
     float _hmdRollControlRate { ROLL_CONTROL_RATE_DEFAULT };
     float _lastDrivenSpeed { 0.0f };
 
-    // working copies -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
+    // working copy -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
     glm::mat4 _sensorToWorldMatrix { glm::mat4() };
 
     // cache of the current HMD sensor position and orientation in sensor space.