From f0608648cbc17bcb6f0387dcbb0d550a9c7a2882 Mon Sep 17 00:00:00 2001
From: Alessio Quaglino <quaglino@google.com>
Date: Sun, 8 Oct 2023 04:03:29 -0700
Subject: [PATCH] Stricter swapping condition in mju_eig3.

This solves the texture rendering issue with the Rubik's cube. The texture were specified with respect to the incorrect frame, so a default rotation of (0, 0, pi/2) is now applied to the model file.

PiperOrigin-RevId: 571715153
Change-Id: I3efae3b1fc8f64e6c9e4db90a7609506e3cbed86
---
 model/cube/cube_3x3x3.xml      | 58 +++++++++++++++++-----------------
 src/engine/engine_util_solve.c |  3 +-
 2 files changed, 31 insertions(+), 30 deletions(-)

diff --git a/model/cube/cube_3x3x3.xml b/model/cube/cube_3x3x3.xml
index e6f63cce..5cc025cf 100644
--- a/model/cube/cube_3x3x3.xml
+++ b/model/cube/cube_3x3x3.xml
@@ -1,5 +1,5 @@
 <mujoco model="Cube 3x3x3">
-  <compiler autolimits="true" angle="radian" texturedir="assets"/>
+  <compiler autolimits="true" texturedir="assets"/>
 
   <option timestep="0.01" integrator="implicitfast"/>
 
@@ -17,7 +17,7 @@
     <motor ctrlrange="-0.05 0.05"/>
     <default class="cubelet">
       <joint type="ball" armature="0.0001" damping="0.0005" frictionloss="0.001"/>
-      <geom type="mesh" condim="1" mesh="cubelet"/>
+      <geom type="mesh" condim="1" mesh="cubelet" euler="0 0 90"/>
     </default>
     <default class="core">
       <geom type="sphere" contype="0" conaffinity="0" group="4" size="0.01"/>
@@ -26,33 +26,33 @@
 
   <asset>
     <texture type="skybox" builtin="gradient" width="512" height="512"/>
-    <texture file="white.png" gridsize="3 4" gridlayout=".....F......" rgb1="0 0 0"/>
-    <texture file="yellow.png" gridsize="3 4" gridlayout=".....B......" rgb1="0 0 0"/>
-    <texture file="red.png" gridsize="3 4" gridlayout=".....D......" rgb1="0 0 0"/>
-    <texture file="orange.png" gridsize="3 4" gridlayout=".....U......" rgb1="0 0 0"/>
-    <texture file="blue.png" gridsize="3 4" gridlayout=".....R......" rgb1="0 0 0"/>
-    <texture file="green.png" gridsize="3 4" gridlayout=".....L......" rgb1="0 0 0"/>
-    <texture file="red_white.png" gridsize="3 4" gridlayout=".....DF....." rgb1="0 0 0"/>
-    <texture file="orange_white.png" gridsize="3 4" gridlayout=".....UF....." rgb1="0 0 0"/>
-    <texture file="blue_white.png" gridsize="3 4" gridlayout=".....RF....." rgb1="0 0 0"/>
-    <texture file="green_white.png" gridsize="3 4" gridlayout=".....LF....." rgb1="0 0 0"/>
-    <texture file="red_yellow.png" gridsize="3 4" gridlayout=".....DB....." rgb1="0 0 0"/>
-    <texture file="orange_yellow.png" gridsize="3 4" gridlayout=".....UB....." rgb1="0 0 0"/>
-    <texture file="blue_yellow.png" gridsize="3 4" gridlayout=".....RB....." rgb1="0 0 0"/>
-    <texture file="green_yellow.png" gridsize="3 4" gridlayout=".....LB....." rgb1="0 0 0"/>
-    <texture file="orange_red.png" gridsize="3 4" gridlayout=".....UD....." rgb1="0 0 0"/>
-    <texture file="blue_red.png" gridsize="3 4" gridlayout=".....RD....." rgb1="0 0 0"/>
-    <texture file="green_red.png" gridsize="3 4" gridlayout=".....LD....." rgb1="0 0 0"/>
-    <texture file="blue_orange.png" gridsize="3 4" gridlayout=".....RU....." rgb1="0 0 0"/>
-    <texture file="green_orange.png" gridsize="3 4" gridlayout=".....LU....." rgb1="0 0 0"/>
-    <texture file="blue_red_white.png" gridsize="3 4" gridlayout=".....RDF...." rgb1="0 0 0"/>
-    <texture file="green_red_white.png" gridsize="3 4" gridlayout=".....LDF...." rgb1="0 0 0"/>
-    <texture file="blue_orange_white.png" gridsize="3 4" gridlayout=".....RUF...." rgb1="0 0 0"/>
-    <texture file="green_orange_white.png" gridsize="3 4" gridlayout=".....LUF...." rgb1="0 0 0"/>
-    <texture file="blue_red_yellow.png" gridsize="3 4" gridlayout=".....RDB...." rgb1="0 0 0"/>
-    <texture file="green_red_yellow.png" gridsize="3 4" gridlayout=".....LDB...." rgb1="0 0 0"/>
-    <texture file="blue_orange_yellow.png" gridsize="3 4" gridlayout=".....RUB...." rgb1="0 0 0"/>
-    <texture file="green_orange_yellow.png" gridsize="3 4" gridlayout=".....LUB...." rgb1="0 0 0"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....F......" file="white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....B......" file="yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....D......" file="red.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....U......" file="orange.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....R......" file="blue.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....L......" file="green.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....DF....." file="red_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....UF....." file="orange_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RF....." file="blue_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LF....." file="green_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....DB....." file="red_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....UB....." file="orange_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RB....." file="blue_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LB....." file="green_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....UD....." file="orange_red.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RD....." file="blue_red.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LD....." file="green_red.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RU....." file="blue_orange.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LU....." file="green_orange.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RDF...." file="blue_red_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LDF...." file="green_red_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RUF...." file="blue_orange_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LUF...." file="green_orange_white.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RDB...." file="blue_red_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LDB...." file="green_red_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....RUB...." file="blue_orange_yellow.png"/>
+    <texture rgb1="0 0 0" gridsize="3 4" gridlayout=".....LUB...." file="green_orange_yellow.png"/>
     <material name="white" texture="white"/>
     <material name="yellow" texture="yellow"/>
     <material name="red" texture="red"/>
diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c
index b93022b6..4efba4a8 100644
--- a/src/engine/engine_util_solve.c
+++ b/src/engine/engine_util_solve.c
@@ -781,7 +781,8 @@ int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum ma
   for (int j=0; j < 3; j++) {
     int j1 = j%2;       // lead index
 
-    if (eigval[j1] < eigval[j1+1]) {
+    // only swap if the eigenvalues are different
+    if (eigval[j1]+eigEPS < eigval[j1+1]) {
       // swap eigenvalues
       t = eigval[j1];
       eigval[j1] = eigval[j1+1];