From 97623f489cd6255fdcae2a2ff7426a639125adb2 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 9 Sep 2016 16:32:57 -0700
Subject: [PATCH] expose setting HMD sensorToWorld from JS script

---
 interface/src/scripting/HMDScriptingInterface.cpp | 15 +++++++++++++++
 interface/src/scripting/HMDScriptingInterface.h   |  9 +++++++--
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/interface/src/scripting/HMDScriptingInterface.cpp b/interface/src/scripting/HMDScriptingInterface.cpp
index b031e05789..aa1b046962 100644
--- a/interface/src/scripting/HMDScriptingInterface.cpp
+++ b/interface/src/scripting/HMDScriptingInterface.cpp
@@ -90,6 +90,16 @@ glm::vec3 HMDScriptingInterface::getPosition() const {
     return glm::vec3();
 }
 
+void HMDScriptingInterface::setPosition(const glm::vec3& position) {
+    MyAvatar* myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+    glm::mat4 hmdToSensor = myAvatar->getHMDSensorMatrix();
+    glm::mat4 sensorToWorld = myAvatar->getSensorToWorldMatrix();
+    glm::mat4 hmdToWorld = sensorToWorld * hmdToSensor;
+    setTranslation(hmdToWorld, position);
+    sensorToWorld = hmdToWorld * glm::inverse(hmdToSensor);
+    myAvatar->setSensorToWorldMatrix(sensorToWorld);
+}
+
 glm::quat HMDScriptingInterface::getOrientation() const {
     if (qApp->getActiveDisplayPlugin()->isHmd()) {
         return glm::normalize(glm::quat_cast(getWorldHMDMatrix()));
@@ -139,3 +149,8 @@ bool HMDScriptingInterface::isKeyboardVisible() {
 void HMDScriptingInterface::centerUI() {
     QMetaObject::invokeMethod(qApp, "centerUI", Qt::QueuedConnection);
 }
+
+void HMDScriptingInterface::snapToAvatar() {
+    MyAvatar* myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+    myAvatar->updateSensorToWorldMatrix();
+}
diff --git a/interface/src/scripting/HMDScriptingInterface.h b/interface/src/scripting/HMDScriptingInterface.h
index 2fbdb76198..45952b3a5e 100644
--- a/interface/src/scripting/HMDScriptingInterface.h
+++ b/interface/src/scripting/HMDScriptingInterface.h
@@ -25,7 +25,7 @@ class QScriptEngine;
 
 class HMDScriptingInterface : public AbstractHMDScriptingInterface, public Dependency {
     Q_OBJECT
-    Q_PROPERTY(glm::vec3 position READ getPosition)
+    Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition)
     Q_PROPERTY(glm::quat orientation READ getOrientation)
     Q_PROPERTY(bool mounted READ isMounted)
 
@@ -58,6 +58,8 @@ public:
     // rotate the overlay UI sphere so that it is centered about the the current HMD position and orientation
     Q_INVOKABLE void centerUI();
 
+    // snap HMD to align with Avatar's current position in world-frame
+    Q_INVOKABLE void snapToAvatar();
 public:
     HMDScriptingInterface();
     static QScriptValue getHUDLookAtPosition2D(QScriptContext* context, QScriptEngine* engine);
@@ -68,7 +70,10 @@ public:
 private:
     // Get the position of the HMD
     glm::vec3 getPosition() const;
-    
+
+    // Set the position of the HMD
+    void setPosition(const glm::vec3& position);
+
     // Get the orientation of the HMD
     glm::quat getOrientation() const;