From a0b122309a124b5faf3f60d8315e8f00913e03fc Mon Sep 17 00:00:00 2001
From: Imanol Fernandez <mortimergoro@gmail.com>
Date: Fri, 10 Jul 2020 22:01:11 +0000
Subject: [PATCH] Bug 1650714 - Part 2: Correct XRRigidTransform inverse math
 r=daoshengmu,kip,lsalzman

Differential Revision: https://phabricator.services.mozilla.com/D82433
---
 dom/vr/XRFrame.cpp          |  5 +++--
 dom/vr/XRRigidTransform.cpp | 16 ++++++----------
 gfx/2d/Quaternion.h         |  6 ++++++
 3 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/dom/vr/XRFrame.cpp b/dom/vr/XRFrame.cpp
index b632ea79c53f..8b1610c91984 100644
--- a/dom/vr/XRFrame.cpp
+++ b/dom/vr/XRFrame.cpp
@@ -79,7 +79,7 @@ already_AddRefed<XRViewerPose> XRFrame::GetViewerPose(
 
     gfx::Matrix4x4Double originTransform;
     originTransform.SetRotationFromQuaternion(
-        aReferenceSpace.GetEffectiveOriginOrientation());
+        aReferenceSpace.GetEffectiveOriginOrientation().Inverse());
     originTransform.PreTranslate(-aReferenceSpace.GetEffectiveOriginPosition());
 
     headTransform *= originTransform;
@@ -155,7 +155,8 @@ already_AddRefed<XRPose> XRFrame::GetPose(const XRSpace& aSpace,
 
   const bool emulatedPosition = aSpace.IsPositionEmulated();
   gfx::Matrix4x4Double base;
-  base.SetRotationFromQuaternion(aBaseSpace.GetEffectiveOriginOrientation());
+  base.SetRotationFromQuaternion(
+      aBaseSpace.GetEffectiveOriginOrientation().Inverse());
   base.PreTranslate(-aBaseSpace.GetEffectiveOriginPosition());
 
   gfx::Matrix4x4Double matrix = aSpace.GetEffectiveOriginTransform() * base;
diff --git a/dom/vr/XRRigidTransform.cpp b/dom/vr/XRRigidTransform.cpp
index 2ef8d6827a19..69af4cd5be21 100644
--- a/dom/vr/XRRigidTransform.cpp
+++ b/dom/vr/XRRigidTransform.cpp
@@ -140,11 +140,9 @@ void XRRigidTransform::UpdateInternal() {
     mOrientation->SetW(mRawOrientation.w);
   }
   if (mInverse) {
-    gfx::QuaternionDouble q(mRawOrientation);
-    gfx::PointDouble3D p = -mRawPosition;
-    p = q.RotatePoint(p);
-    q.Invert();
-    mInverse->Update(p, q);
+    gfx::Matrix4x4Double inverseMatrix = mRawTransformMatrix;
+    Unused << inverseMatrix.Invert();
+    mInverse->Update(inverseMatrix);
   }
 }
 
@@ -175,11 +173,9 @@ void XRRigidTransform::GetMatrix(JSContext* aCx,
 
 already_AddRefed<XRRigidTransform> XRRigidTransform::Inverse() {
   if (!mInverse) {
-    gfx::QuaternionDouble q(mRawOrientation);
-    gfx::PointDouble3D p = -mRawPosition;
-    p = q.RotatePoint(p);
-    q.Invert();
-    mInverse = new XRRigidTransform(mParent, p, q);
+    gfx::Matrix4x4Double inverseMatrix = mRawTransformMatrix;
+    Unused << inverseMatrix.Invert();
+    mInverse = new XRRigidTransform(mParent, inverseMatrix);
   }
 
   RefPtr<XRRigidTransform> inverse = mInverse;
diff --git a/gfx/2d/Quaternion.h b/gfx/2d/Quaternion.h
index 7bc47db92b0c..a952612b042c 100644
--- a/gfx/2d/Quaternion.h
+++ b/gfx/2d/Quaternion.h
@@ -122,6 +122,12 @@ class BaseQuaternion {
 
   BaseQuaternion& Invert() { return Conjugate().Normalize(); }
 
+  BaseQuaternion Inverse() const {
+    BaseQuaternion q = *this;
+    q.Invert();
+    return q;
+  }
+
   Point3DTyped<UnknownUnits, T> RotatePoint(
       const Point3DTyped<UnknownUnits, T>& aPoint) const {
     T uvx = T(2.0) * (y * aPoint.z - z * aPoint.y);