Bug 1650714 - Part 1: Correct Matrix Decompose and SetRotationFromQuaternion. r=kip,imanol,lsalzman

Differential Revision: https://phabricator.services.mozilla.com/D82391

dom/vr/XRFrame.cpp

@@ -73,10 +73,6 @@ already_AddRefed<XRViewerPose> XRFrame::GetViewerPose(
         sensorState.pose.orientation[0], sensorState.pose.orientation[1],
         sensorState.pose.orientation[2], sensorState.pose.orientation[3]);
 
-    // Quaternion was inverted for WebVR. We need to invert it here again.
-    // TODO: Remove those extra inverts when WebVR support is disabled.
-    viewerOrientation.Invert();
-
     gfx::Matrix4x4Double headTransform;
     headTransform.SetRotationFromQuaternion(viewerOrientation);
     headTransform.PostTranslate(viewerPosition);

dom/vr/XRNativeOriginTracker.cpp

@@ -25,10 +25,6 @@ gfx::QuaternionDouble XRNativeOriginTracker::GetOrientation() {
   gfx::QuaternionDouble orientation(
       mPose->orientation[0], mPose->orientation[1], mPose->orientation[2],
       mPose->orientation[3]);
-  // Quaternion was inverted for WebVR in XXXVRSession when handling controller
-  // poses. We need to re-invert it here again.
-  // TODO: Remove those extra inverts when WebVR support is disabled.
-  orientation.Invert();
   return orientation;
 }
 

dom/vr/XRRigidTransform.cpp

@@ -60,10 +60,6 @@ XRRigidTransform::XRRigidTransform(nsISupports* aParent,
   gfx::PointDouble3D scale;
   mRawTransformMatrix = aTransform;
   mRawTransformMatrix.Decompose(mRawPosition, mRawOrientation, scale);
-  // TODO: Investigate why we need to do this invert after getting orientation
-  // from the transform matrix. It looks like we have a bug at
-  // Matrix4x4Typed.SetFromRotationMatrix() (Bug 1635363).
-  mRawOrientation.Invert();
 }
 
 XRRigidTransform::~XRRigidTransform() { mozilla::DropJSObjects(this); }
@@ -128,10 +124,6 @@ void XRRigidTransform::Update(const gfx::Matrix4x4Double& aTransform) {
   mRawTransformMatrix = aTransform;
   gfx::PointDouble3D scale;
   mRawTransformMatrix.Decompose(mRawPosition, mRawOrientation, scale);
-  // TODO: Investigate why we need to do this invert after getting orientation
-  // from the transform matrix. It looks like we have a bug at
-  // Matrix4x4Typed.SetFromRotationMatrix() (Bug 1635363).
-  mRawOrientation.Invert();
   UpdateInternal();
 }
 

gfx/2d/Matrix.h

@@ -1433,21 +1433,9 @@ class Matrix4x4Typed {
       // We do not support matrices with a zero scale component
       return false;
     }
-    T invXS = 1.0f / scale.x;
-    T invYS = 1.0f / scale.y;
-    T invZS = 1.0f / scale.z;
-    mat._11 *= invXS;
-    mat._21 *= invXS;
-    mat._31 *= invXS;
-    mat._12 *= invYS;
-    mat._22 *= invYS;
-    mat._32 *= invYS;
-    mat._13 *= invZS;
-    mat._23 *= invZS;
-    mat._33 *= invZS;
 
     // Extract rotation
-    rotation.SetFromRotationMatrix(mat);
+    rotation.SetFromRotationMatrix(*this);
     return true;
   }
 
@@ -1461,17 +1449,17 @@ class Matrix4x4Typed {
     const T wx = q.w * x2, wy = q.w * y2, wz = q.w * z2;
 
     _11 = 1.0f - (yy + zz);
-    _21 = xy + wz;
-    _31 = xz - wy;
+    _21 = xy - wz;
+    _31 = xz + wy;
     _41 = 0.0f;
 
-    _12 = xy - wz;
+    _12 = xy + wz;
     _22 = 1.0f - (xx + zz);
-    _32 = yz + wx;
+    _32 = yz - wx;
     _42 = 0.0f;
 
-    _13 = xz + wy;
-    _23 = yz - wx;
+    _13 = xz - wy;
+    _23 = yz + wx;
     _33 = 1.0f - (xx + yy);
     _43 = 0.0f;
 

gfx/2d/Quaternion.h

@@ -48,34 +48,35 @@ class BaseQuaternion {
   // Assumes upper 3x3 of aMatrix is a pure rotation matrix (no scaling)
   void SetFromRotationMatrix(
       const Matrix4x4Typed<UnknownUnits, UnknownUnits, T>& m) {
-    // see
-    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
-    const T trace = m._11 + m._22 + m._33;
-    if (trace > 0.0) {
-      const T s = 0.5f / sqrt(trace + 1.0f);
+    const T trace = m._11 + m._22 + m._33 + 1.0f;
+
+    if (trace > 1e-4) {
+      const T s = 0.5f / sqrt(trace);
       w = 0.25f / s;
-      x = (m._32 - m._23) * s;
-      y = (m._13 - m._31) * s;
-      z = (m._21 - m._12) * s;
+      x = (m._23 - m._32) * s;
+      y = (m._31 - m._13) * s;
+      z = (m._12 - m._21) * s;
     } else if (m._11 > m._22 && m._11 > m._33) {
       const T s = 2.0f * sqrt(1.0f + m._11 - m._22 - m._33);
-      w = (m._32 - m._23) / s;
+      w = (m._23 - m._32) / s;
       x = 0.25f * s;
-      y = (m._12 + m._21) / s;
-      z = (m._13 + m._31) / s;
+      y = (m._21 + m._12) / s;
+      z = (m._31 + m._13) / s;
     } else if (m._22 > m._33) {
       const T s = 2.0 * sqrt(1.0f + m._22 - m._11 - m._33);
-      w = (m._13 - m._31) / s;
-      x = (m._12 + m._21) / s;
+      w = (m._31 - m._13) / s;
+      x = (m._21 + m._12) / s;
       y = 0.25f * s;
-      z = (m._23 + m._32) / s;
+      z = (m._32 + m._23) / s;
     } else {
       const T s = 2.0 * sqrt(1.0f + m._33 - m._11 - m._22);
-      w = (m._21 - m._12) / s;
-      x = (m._13 + m._31) / s;
-      y = (m._23 + m._32) / s;
+      w = (m._12 - m._21) / s;
+      x = (m._31 + m._13) / s;
+      y = (m._32 + m._23) / s;
       z = 0.25f * s;
     }
+
+    Normalize();
   }
 
   // result = this * aQuat

gfx/vr/VRDisplayClient.cpp

@@ -258,9 +258,7 @@ void VRDisplayClient::GamepadMappingForWebVR(
                            aControllerState.pose.orientation[1],
                            aControllerState.pose.orientation[2],
                            aControllerState.pose.orientation[3]);
-      // We need to invert its quaternion here because we did an invertion
-      // in FxR WaveVR delegate.
-      originalMtx.SetRotationFromQuaternion(quat.Invert());
+      originalMtx.SetRotationFromQuaternion(quat);
       originalMtx._41 = aControllerState.pose.position[0];
       originalMtx._42 = aControllerState.pose.position[1];
       originalMtx._43 = aControllerState.pose.position[2];
@@ -269,7 +267,6 @@ void VRDisplayClient::GamepadMappingForWebVR(
       gfx::Point3D pos, scale;
       originalMtx.Decompose(pos, quat, scale);
 
-      quat.Invert();
       aControllerState.pose.position[0] = pos.x;
       aControllerState.pose.position[1] = pos.y;
       aControllerState.pose.position[2] = pos.z;
@@ -299,9 +296,7 @@ void VRDisplayClient::GamepadMappingForWebVR(
                            aControllerState.pose.orientation[1],
                            aControllerState.pose.orientation[2],
                            aControllerState.pose.orientation[3]);
-      // We need to invert its quaternion here because we did an invertion
-      // in FxR OculusVR delegate.
-      originalMtx.SetRotationFromQuaternion(quat.Invert());
+      originalMtx.SetRotationFromQuaternion(quat);
       originalMtx._41 = aControllerState.pose.position[0];
       originalMtx._42 = aControllerState.pose.position[1];
       originalMtx._43 = aControllerState.pose.position[2];
@@ -310,7 +305,6 @@ void VRDisplayClient::GamepadMappingForWebVR(
       gfx::Point3D pos, scale;
       originalMtx.Decompose(pos, quat, scale);
 
-      quat.Invert();
       aControllerState.pose.position[0] = pos.x;
       aControllerState.pose.position[1] = pos.y;
       aControllerState.pose.position[2] = pos.z;
@@ -365,9 +359,7 @@ void VRDisplayClient::GamepadMappingForWebVR(
                            aControllerState.pose.orientation[1],
                            aControllerState.pose.orientation[2],
                            aControllerState.pose.orientation[3]);
-      // We need to invert its quaternion here because we did an invertion
-      // in FxR OculusVR delegate.
-      originalMtx.SetRotationFromQuaternion(quat.Invert());
+      originalMtx.SetRotationFromQuaternion(quat);
       originalMtx._41 = aControllerState.pose.position[0];
       originalMtx._42 = aControllerState.pose.position[1];
       originalMtx._43 = aControllerState.pose.position[2];
@@ -376,7 +368,6 @@ void VRDisplayClient::GamepadMappingForWebVR(
       gfx::Point3D pos, scale;
       originalMtx.Decompose(pos, quat, scale);
 
-      quat.Invert();
       aControllerState.pose.position[0] = pos.x;
       aControllerState.pose.position[1] = pos.y;
       aControllerState.pose.position[2] = pos.z;