// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
//   used to endorse or promote products derived from this software without
//   specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// Tests the use of Cere's Jet type with the quaternions found in util/math/. In
// theory, the unittests for the quaternion class should be type parameterized
// to make for easier testing of instantiations of the quaternion class, but it
// is not so, and not obviously worth the work to make the switch at this time.

#include "base/stringprintf.h"
#include "gtest/gtest.h"
#include "util/math/mathlimits.h"
#include "util/math/matrix3x3.h"
#include "util/math/quaternion.h"
#include "util/math/vector3.h"
#include "ceres/test_util.h"
#include "ceres/jet.h"
#include "ceres/jet_traits.h"

namespace ceres {
namespace internal {

// Use a 4-element derivative to simulate the case where each of the
// quaternion elements are derivative parameters.
typedef Jet<double, 4> J;

struct JetTraitsTest : public ::testing::Test {
 protected:
  JetTraitsTest()
      : a(J(1.1, 0), J(2.1, 1), J(3.1, 2), J(4.1, 3)),
        b(J(0.1, 0), J(1.1, 1), J(2.1, 2), J(5.0, 3)),
        double_a(a[0].a, a[1].a, a[2].a, a[3].a),
        double_b(b[0].a, b[1].a, b[2].a, b[3].a) {
    // The quaternions should be valid rotations, so normalize them.
    a.Normalize();
    b.Normalize();
    double_a.Normalize();
    double_b.Normalize();
  }

  virtual ~JetTraitsTest() {}

  // A couple of arbitrary normalized quaternions.
  Quaternion<J> a, b;

  // The equivalent of a, b but in scalar form.
  Quaternion<double> double_a, double_b;
};

// Compare scalar multiplication to jet multiplication. Ignores derivatives.
TEST_F(JetTraitsTest, QuaternionScalarMultiplicationWorks) {
  Quaternion<J> c = a * b;
  Quaternion<double> double_c = double_a * double_b;

  for (int i = 0; i < 4; ++i) {
    EXPECT_EQ(double_c[i], c[i].a);
  }
}

// Compare scalar slerp to jet slerp. Ignores derivatives.
TEST_F(JetTraitsTest, QuaternionScalarSlerpWorks) {
  const J fraction(0.1);
  Quaternion<J> c = Quaternion<J>::Slerp(a, b, fraction);
  Quaternion<double> double_c =
      Quaternion<double>::Slerp(double_a, double_b, fraction.a);

  for (int i = 0; i < 4; ++i) {
    EXPECT_EQ(double_c[i], c[i].a);
  }
}

// On a 32-bit optimized build, the mismatch is about 1.4e-14.
double const kTolerance = 1e-13;

void ExpectJetsClose(const J &x, const J &y) {
  ExpectClose(x.a, y.a, kTolerance);
  ExpectClose(x.v[0], y.v[0], kTolerance);
  ExpectClose(x.v[1], y.v[1], kTolerance);
  ExpectClose(x.v[2], y.v[2], kTolerance);
  ExpectClose(x.v[3], y.v[3], kTolerance);
}

void ExpectQuaternionsClose(const Quaternion<J>& x, const Quaternion<J>& y) {
  for (int i = 0; i < 4; ++i) {
    ExpectJetsClose(x[i], y[i]);
  }
}

// Compare jet slurp to jet slerp using identies, checking derivatives.
TEST_F(JetTraitsTest, CheckSlerpIdentitiesWithNontrivialDerivatives) {
  // Do a slerp to 0.75 directly.
  Quaternion<J> direct = Quaternion<J>::Slerp(a, b, J(0.75));

  // Now go part way twice, in theory ending at the same place.
  Quaternion<J> intermediate = Quaternion<J>::Slerp(a, b, J(0.5));
  Quaternion<J> indirect = Quaternion<J>::Slerp(intermediate, b, J(0.5));

  // Check that the destination is the same, including derivatives.
  ExpectQuaternionsClose(direct, indirect);
}

TEST_F(JetTraitsTest, CheckAxisAngleIsInvertibleWithNontrivialDerivatives) {
  Vector3<J> axis;
  J angle;
  a.GetAxisAngle(&axis, &angle);
  b.SetFromAxisAngle(axis, angle);

  ExpectQuaternionsClose(a, b);
}

TEST_F(JetTraitsTest,
       CheckRotationMatrixIsInvertibleWithNontrivialDerivatives) {
  Vector3<J> axis;
  J angle;
  Matrix3x3<J> R;
  a.ToRotationMatrix(&R);
  b.SetFromRotationMatrix(R);

  ExpectQuaternionsClose(a, b);
}

// This doesn't check correctnenss, only that the instantiation compiles.
TEST_F(JetTraitsTest, CheckRotationBetweenIsCompilable) {
  // Get two arbitrary vectors x and y.
  Vector3<J> x, y;
  J ignored_angle;
  a.GetAxisAngle(&x, &ignored_angle);
  b.GetAxisAngle(&y, &ignored_angle);

  Quaternion<J> between_x_and_y = Quaternion<J>::RotationBetween(x, y);

  // Prevent optimizing this away.
  EXPECT_NE(between_x_and_y[0].a, 0.0);
}

TEST_F(JetTraitsTest, CheckRotatedWorksAsExpected) {
  // Get two arbitrary vectors x and y.
  Vector3<J> x;
  J ignored_angle;
  a.GetAxisAngle(&x, &ignored_angle);

  // Rotate via a quaternion.
  Vector3<J> y = b.Rotated(x);

  // Rotate via a rotation matrix.
  Matrix3x3<J> R;
  b.ToRotationMatrix(&R);
  Vector3<J> yp = R * x;

  ExpectJetsClose(yp[0], y[0]);
  ExpectJetsClose(yp[1], y[1]);
  ExpectJetsClose(yp[2], y[2]);
}

TEST_F(JetTraitsTest, CheckRotatedWorksAsExpectedWithDoubles) {
  // Get two arbitrary vectors x and y.
  Vector3<double> x;
  double ignored_angle;
  double_a.GetAxisAngle(&x, &ignored_angle);

  // Rotate via a quaternion.
  Vector3<double> y = double_b.Rotated(x);

  // Rotate via a rotation matrix.
  Matrix3x3<double> R;
  double_b.ToRotationMatrix(&R);
  Vector3<double> yp = R * x;

  ExpectClose(yp[0], y[0], kTolerance);
  ExpectClose(yp[1], y[1], kTolerance);
  ExpectClose(yp[2], y[2], kTolerance);
}

}  // namespace internal
}  // namespace ceres