content: package com.google.vrtoolkit.cardboard.sensors.internal;
  
  public class So3Helper {
      private static final double M_SQRT1_2 = 0.7071067811865476;
      private static final double ONE_6TH = 0.1666666716337204;
      private static final double ONE_20TH = 0.1666666716337204;
      private Vector3d temp31;
      private Vector3d sO3FromTwoVecN;
      private Vector3d sO3FromTwoVecA;
      private Vector3d sO3FromTwoVecB;
      private Vector3d sO3FromTwoVecRotationAxis;
      private Matrix3x3d sO3FromTwoVec33R1;
      private Matrix3x3d sO3FromTwoVec33R2;
      private Vector3d muFromSO3R2;
      private Vector3d rotationPiAboutAxisTemp;
  
      public So3Helper() {
          temp31 = new Vector3d();
          sO3FromTwoVecN = new Vector3d();
          sO3FromTwoVecA = new Vector3d();
          sO3FromTwoVecB = new Vector3d();
          sO3FromTwoVecRotationAxis = new Vector3d();
          sO3FromTwoVec33R1 = new Matrix3x3d();
          sO3FromTwoVec33R2 = new Matrix3x3d();
          muFromSO3R2 = new Vector3d();
          rotationPiAboutAxisTemp = new Vector3d();
      }
  
      public void sO3FromTwoVec(final Vector3d a, final Vector3d b, final Matrix3x3d result) {
          Vector3d.cross(a, b, sO3FromTwoVecN);
          if (sO3FromTwoVecN.length() == 0.0) {
              final double dot = Vector3d.dot(a, b);
              if (dot >= 0.0) {
                  result.setIdentity();
              }
              else {
                  Vector3d.ortho(a, sO3FromTwoVecRotationAxis);
                  rotationPiAboutAxis(sO3FromTwoVecRotationAxis, result);
              }
              return;
          }
          sO3FromTwoVecA.set(a);
          sO3FromTwoVecB.set(b);
          sO3FromTwoVecN.normalize();
          sO3FromTwoVecA.normalize();
          sO3FromTwoVecB.normalize();
          final Matrix3x3d r1 = sO3FromTwoVec33R1;
          r1.setColumn(0, sO3FromTwoVecA);
          r1.setColumn(1, sO3FromTwoVecN);
          Vector3d.cross(sO3FromTwoVecN, sO3FromTwoVecA, temp31);
          r1.setColumn(2, temp31);
          final Matrix3x3d r2 = sO3FromTwoVec33R2;
          r2.setColumn(0, sO3FromTwoVecB);
          r2.setColumn(1, sO3FromTwoVecN);
          Vector3d.cross(sO3FromTwoVecN, sO3FromTwoVecB, temp31);
          r2.setColumn(2, temp31);
          r1.transpose();
          Matrix3x3d.mult(r2, r1, result);
      }
      
      private void rotationPiAboutAxis(final Vector3d v, final Matrix3x3d result) {
          rotationPiAboutAxisTemp.set(v);
          rotationPiAboutAxisTemp.scale(3.141592653589793 / rotationPiAboutAxisTemp.length());
          final double invTheta = 0.3183098861837907;
          final double kA = 0.0;
          final double kB = 0.20264236728467558;
          rodriguesSo3Exp(rotationPiAboutAxisTemp, kA, kB, result);
      }
      
      public void muFromSO3(final Matrix3x3d so3, final Vector3d result) {
          final double cosAngle = (so3.get(0, 0) + so3.get(1, 1) + so3.get(2, 2) - 1.0) * 0.5;
          result.set((so3.get(2, 1) - so3.get(1, 2)) / 2.0, (so3.get(0, 2) - so3.get(2, 0)) / 2.0, (so3.get(1, 0) - so3.get(0, 1)) / 2.0);
          final double sinAngleAbs = result.length();
          if (cosAngle > 0.7071067811865476) {
              if (sinAngleAbs > 0.0) {
                  result.scale(Math.asin(sinAngleAbs) / sinAngleAbs);
              }
          }
          else if (cosAngle > -0.7071067811865476) {
              final double angle = Math.acos(cosAngle);
              result.scale(angle / sinAngleAbs);
          }
          else {
              final double angle = 3.141592653589793 - Math.asin(sinAngleAbs);
              final double d0 = so3.get(0, 0) - cosAngle;
              final double d = so3.get(1, 1) - cosAngle;
              final double d2 = so3.get(2, 2) - cosAngle;
              final Vector3d r2 = muFromSO3R2;
              if (d0 * d0 > d * d && d0 * d0 > d2 * d2) {
                  r2.set(d0, (so3.get(1, 0) + so3.get(0, 1)) / 2.0, (so3.get(0, 2) + so3.get(2, 0)) / 2.0);
              }
              else if (d * d > d2 * d2) {
                  r2.set((so3.get(1, 0) + so3.get(0, 1)) / 2.0, d, (so3.get(2, 1) + so3.get(1, 2)) / 2.0);
              }
              else {
                  r2.set((so3.get(0, 2) + so3.get(2, 0)) / 2.0, (so3.get(2, 1) + so3.get(1, 2)) / 2.0, d2);
              }
              if (Vector3d.dot(r2, result) < 0.0) {
                  r2.scale(-1.0);
              }
              r2.normalize();
              r2.scale(angle);
              result.set(r2);
          }
      }
      
      private static void rodriguesSo3Exp(final Vector3d w, final double kA, final double kB, final Matrix3x3d result) {
          final double wx2 = w.x * w.x;
          final double wy2 = w.y * w.y;
          final double wz2 = w.z * w.z;
          result.set(0, 0, 1.0 - kB * (wy2 + wz2));
          result.set(1, 1, 1.0 - kB * (wx2 + wz2));
          result.set(2, 2, 1.0 - kB * (wx2 + wy2));
          double a = kA * w.z;
          double b = kB * (w.x * w.y);
          result.set(0, 1, b - a);
          result.set(1, 0, b + a);
          a = kA * w.y;
          b = kB * (w.x * w.z);
          result.set(0, 2, b + a);
          result.set(2, 0, b - a);
          a = kA * w.x;
          b = kB * (w.y * w.z);
          result.set(1, 2, b - a);
          result.set(2, 1, b + a);
      }
  }
  