From f0912987a384dc33c289c4f00718b5c7b4816513 Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Tue, 5 Sep 2017 10:03:19 -0700
Subject: [PATCH] Fix ray picks against HUD sphere at different sensor scale
 factors

---
 interface/src/Application.cpp                 |  2 +-
 .../src/raypick/RayPickScriptingInterface.cpp |  2 +-
 .../src/display-plugins/CompositorHelper.cpp  | 25 ++++++++++++-------
 .../src/display-plugins/CompositorHelper.h    |  6 ++++-
 4 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 458026c6a8..79885cc6b6 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2551,7 +2551,7 @@ void Application::paintGL() {
 
     {
         PROFILE_RANGE(render, "/updateCompositor");
-        getApplicationCompositor().setFrameInfo(_frameCount, _myCamera.getTransform());
+        getApplicationCompositor().setFrameInfo(_frameCount, _myCamera.getTransform(), getMyAvatar()->getSensorToWorldMatrix());
     }
 
     gpu::FramebufferPointer finalFramebuffer;
diff --git a/interface/src/raypick/RayPickScriptingInterface.cpp b/interface/src/raypick/RayPickScriptingInterface.cpp
index 015f8fd7d1..cb2b3e4471 100644
--- a/interface/src/raypick/RayPickScriptingInterface.cpp
+++ b/interface/src/raypick/RayPickScriptingInterface.cpp
@@ -108,4 +108,4 @@ void RayPickScriptingInterface::setIgnoreAvatars(QUuid uid, const QScriptValue&
 
 void RayPickScriptingInterface::setIncludeAvatars(QUuid uid, const QScriptValue& includeAvatars) {
     qApp->getRayPickManager().setIncludeAvatars(uid, includeAvatars);
-}
\ No newline at end of file
+}
diff --git a/libraries/display-plugins/src/display-plugins/CompositorHelper.cpp b/libraries/display-plugins/src/display-plugins/CompositorHelper.cpp
index f09b3d7109..2f57cc29d0 100644
--- a/libraries/display-plugins/src/display-plugins/CompositorHelper.cpp
+++ b/libraries/display-plugins/src/display-plugins/CompositorHelper.cpp
@@ -20,6 +20,7 @@
 #include <QtGui/QWindow>
 #include <QQuickWindow>
 
+#include <DebugDraw.h>
 #include <shared/QtHelpers.h>
 #include <ui/Menu.h>
 #include <NumericalConstants.h>
@@ -339,23 +340,29 @@ void CompositorHelper::computeHmdPickRay(const glm::vec2& cursorPos, glm::vec3&
 glm::mat4 CompositorHelper::getUiTransform() const {
     glm::mat4 modelMat;
     _modelTransform.getMatrix(modelMat);
-    return _currentCamera * glm::inverse(_currentDisplayPlugin->getHeadPose()) * modelMat;
+    return _sensorToWorldMatrix * modelMat;
 }
 
 //Finds the collision point of a world space ray
 bool CompositorHelper::calculateRayUICollisionPoint(const glm::vec3& position, const glm::vec3& direction, glm::vec3& result) const {
-    auto UITransform = getUiTransform();
-    auto relativePosition4 = glm::inverse(UITransform) * vec4(position, 1);
-    auto relativePosition = vec3(relativePosition4) / relativePosition4.w;
-    auto relativeDirection = glm::inverse(glm::quat_cast(UITransform)) * direction;
+    glm::mat4 uiToWorld = getUiTransform();
+    glm::mat4 worldToUi = glm::inverse(uiToWorld);
+    glm::vec3 localPosition = transformPoint(worldToUi, position);
+    glm::vec3 localDirection = glm::normalize(transformVectorFast(worldToUi, direction));
 
-    const float UI_RADIUS = 1.0f; // * myAvatar->getUniformScale(); // FIXME - how do we want to handle avatar scale
+    const float UI_RADIUS = 1.0f;
     float instersectionDistance;
-    if (raySphereIntersect(relativeDirection, relativePosition, UI_RADIUS, &instersectionDistance)){
-        result = position + glm::normalize(direction) * instersectionDistance;
+    if (raySphereIntersect(localDirection, localPosition, UI_RADIUS, &instersectionDistance)) {
+        result = transformPoint(uiToWorld, localPosition + localDirection * instersectionDistance);
+#ifdef WANT_DEBUG
+        DebugDraw::getInstance().drawRay(position, result, glm::vec4(0.0f, 1.0f, 0.0f, 1.0f));
+#endif
         return true;
+    } else {
+#ifdef WANT_DEBUG
+        DebugDraw::getInstance().drawRay(position, position + (direction * 1000.0f), glm::vec4(1.0f, 0.0f, 0.0f, 1.0f));
+#endif
     }
-
     return false;
 }
 
diff --git a/libraries/display-plugins/src/display-plugins/CompositorHelper.h b/libraries/display-plugins/src/display-plugins/CompositorHelper.h
index 946229fc9b..b1d2815f65 100644
--- a/libraries/display-plugins/src/display-plugins/CompositorHelper.h
+++ b/libraries/display-plugins/src/display-plugins/CompositorHelper.h
@@ -109,7 +109,10 @@ public:
     void setReticleOverDesktop(bool value) { _isOverDesktop = value; }
 
     void setDisplayPlugin(const DisplayPluginPointer& displayPlugin) { _currentDisplayPlugin = displayPlugin; }
-    void setFrameInfo(uint32_t frame, const glm::mat4& camera) { _currentCamera = camera; }
+    void setFrameInfo(uint32_t frame, const glm::mat4& camera, const glm::mat4& sensorToWorldMatrix) {
+        _currentCamera = camera;
+        _sensorToWorldMatrix = sensorToWorldMatrix;
+    }
 
 signals:
     void allowMouseCaptureChanged();
@@ -124,6 +127,7 @@ private:
 
     DisplayPluginPointer _currentDisplayPlugin;
     glm::mat4 _currentCamera;
+    glm::mat4 _sensorToWorldMatrix;
     QWidget* _renderingWidget{ nullptr };
 
     //// Support for hovering and tooltips