From 575b34918f067704d797789a7aae588ab6831ec0 Mon Sep 17 00:00:00 2001
From: Brent Yi <yibrenth@gmail.com>
Date: Sun, 24 Dec 2023 22:07:03 -0800
Subject: [PATCH] Brute-force fix for extra degree-of-freedom when setting up
 direction

---
 src/viser/_message_api.py    | 23 +++++++++++++++++------
 src/viser/client/src/App.tsx |  4 ++--
 2 files changed, 19 insertions(+), 8 deletions(-)

diff --git a/src/viser/_message_api.py b/src/viser/_message_api.py
index 069b289db..0e430d80b 100644
--- a/src/viser/_message_api.py
+++ b/src/viser/_message_api.py
@@ -295,12 +295,23 @@ def rotate_between(before: onp.ndarray, after: onp.ndarray) -> tf.SO3:
 
         R_threeworld_world = rotate_between(default_three_up, direction)
 
-        # If we set +Y to up, +X and +Z should face the camera.
-        # If we set +Z to up, +X and +Y should face the camera.
-        forward_wrt_world = R_threeworld_world.inverse() @ onp.array([1.0, 0.0, 1.0])
-        R_threeworld_world = R_threeworld_world @ rotate_between(
-            forward_wrt_world, onp.abs(forward_wrt_world)
-        )
+        # Rotate the world frame such that:
+        #     If we set +Y to up, +X and +Z should face the camera.
+        #     If we set +Z to up, +X and +Y should face the camera.
+        #
+        # This could be made more efficient...
+        thetas = onp.arange(360)
+        sums = [
+            onp.sum(
+                (tf.SO3.from_y_radians(theta) @ R_threeworld_world)
+                @ onp.array([-1.0, -1.0, -1.0])
+            )
+            for theta in thetas
+        ]
+        best_theta = thetas[onp.argmax(sums)]
+
+        R_threeworld_world = tf.SO3.from_y_radians(best_theta) @ R_threeworld_world
+
         if not onp.any(onp.isnan(R_threeworld_world.wxyz)):
             # Set the orientation of the root node.
             self._queue(
diff --git a/src/viser/client/src/App.tsx b/src/viser/client/src/App.tsx
index ff9e24da6..7ef17f2a6 100644
--- a/src/viser/client/src/App.tsx
+++ b/src/viser/client/src/App.tsx
@@ -124,7 +124,7 @@ function ViewerRoot() {
       "": {
         wxyz: (() => {
           const quat = new THREE.Quaternion().setFromEuler(
-            new THREE.Euler(Math.PI / 2, Math.PI, Math.PI / 2),
+            new THREE.Euler(Math.PI / 2, Math.PI, -Math.PI / 2),
           );
           return [quat.w, quat.x, quat.y, quat.z];
         })(),
@@ -213,7 +213,7 @@ function ViewerCanvas({ children }: { children: React.ReactNode }) {
   );
   return (
     <Canvas
-      camera={{ position: [3.0, 3.0, 3.0] }}
+      camera={{ position: [-3.0, 3.0, -3.0] }}
       gl={{ preserveDrawingBuffer: true }}
       style={{
         position: "relative",