/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  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 Open Source Robotics Foundation 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 Jia Pan */

#include <array>
#include <iostream>
#include <limits>

#include <gtest/gtest.h>

#include "fcl/common/unused.h"

#include "fcl/math/motion/translation_motion.h"

#include "fcl/geometry/shape/cone.h"
#include "fcl/geometry/shape/capsule.h"
#include "fcl/geometry/shape/ellipsoid.h"
#include "fcl/geometry/shape/halfspace.h"
#include "fcl/geometry/shape/plane.h"

#include "fcl/narrowphase/detail/gjk_solver_indep.h"
#include "fcl/narrowphase/detail/gjk_solver_libccd.h"
#include "fcl/narrowphase/collision.h"

#include "test_fcl_utility.h"

using namespace fcl;

template <typename S>
std::array<S, 6>& extents()
{
  static std::array<S, 6> static_extents{ {0, 0, 0, 10, 10, 10} };
  return static_extents;
}

template <typename S>
detail::GJKSolver_libccd<S>& solver1()
{
  static detail::GJKSolver_libccd<S> static_solver1;
  return static_solver1;
}

template <typename S>
detail::GJKSolver_indep<S>& solver2()
{
  static detail::GJKSolver_indep<S> static_solver2;
  return static_solver2;
}

template <typename S>
S tolerance();

template <>
float tolerance() { return 1e-4; }

template <>
double tolerance() { return 1e-12; }

template <typename S>
void test_sphere_shape()
{
  const S radius = 5.0;
  const S pi = constants<S>::pi();

  Sphere<S> s(radius);

  const auto volume = 4.0 / 3.0 * pi * radius * radius * radius;
  EXPECT_NEAR(volume, s.computeVolume(), tolerance<S>());
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, sphere_shape)
{
//  test_sphere_shape<float>();
  test_sphere_shape<double>();
}

template <typename S>
void test_gjkcache()
{
  Cylinder<S> s1(5, 10);
  Cone<S> s2(5, 10);

  CollisionRequest<S> request;
  request.enable_cached_gjk_guess = true;
  request.gjk_solver_type = GST_INDEP;

  TranslationMotion<S> motion(Transform3<S>(Translation3<S>(Vector3<S>(-20.0, -20.0, -20.0))), Transform3<S>(Translation3<S>(Vector3<S>(20.0, 20.0, 20.0))));

  int N = 1000;
  S dt = 1.0 / (N - 1);

  /// test exploiting spatial coherence
  test::Timer timer1;
  timer1.start();
  std::vector<bool> result1(N);
  for(int i = 0; i < N; ++i)
  {
    motion.integrate(dt * i);
    Transform3<S> tf;
    motion.getCurrentTransform(tf);

    CollisionResult<S> result;

    collide(&s1, Transform3<S>::Identity(), &s2, tf, request, result);
    result1[i] = result.isCollision();
    request.cached_gjk_guess = result.cached_gjk_guess; // use cached guess
  }

  timer1.stop();

  /// test without exploiting spatial coherence
  test::Timer timer2;
  timer2.start();
  std::vector<bool> result2(N);
  request.enable_cached_gjk_guess = false;
  for(int i = 0; i < N; ++i)
  {
    motion.integrate(dt * i);
    Transform3<S> tf;
    motion.getCurrentTransform(tf);

    CollisionResult<S> result;

    collide(&s1, Transform3<S>::Identity(), &s2, tf, request, result);
    result2[i] = result.isCollision();
  }

  timer2.stop();

  std::cout << timer1.getElapsedTime() << " " << timer2.getElapsedTime() << std::endl;

  for(std::size_t i = 0; i < result1.size(); ++i)
  {
    EXPECT_TRUE(result1[i] == result2[i]);
  }
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, gjkcache)
{
//  test_gjkcache<float>();
  test_gjkcache<double>();
}

template <typename Shape1, typename Shape2>
void printComparisonError(const std::string& comparison_type,
                          const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
                          const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
                          GJKSolverType solver_type,
                          const Vector3<typename Shape1::S>& expected_contact_or_normal,
                          const Vector3<typename Shape1::S>& actual_contact_or_normal,
                          bool check_opposite_normal,
                          typename Shape1::S tol)
{
  std::cout << "Disagreement between " << comparison_type
            << " and expected_" << comparison_type << " for "
            << test::getNodeTypeName(s1.getNodeType()) << " and "
            << test::getNodeTypeName(s2.getNodeType()) << " with '"
            << test::getGJKSolverName(solver_type) << "' solver." << std::endl
            << "tf1.linear: \n" << tf1.linear() << std::endl
            << "tf1.translation: " << tf1.translation().transpose() << std::endl
            << "tf2.linear: \n" << tf2.linear() << std::endl
            << "tf2.translation: " << tf2.translation().transpose() << std::endl
            << "expected_" << comparison_type << ": " << expected_contact_or_normal
            << "actual_" << comparison_type << "  : " << actual_contact_or_normal << std::endl;

  if (check_opposite_normal)
    std::cout << " or " << -expected_contact_or_normal;

  std::cout << std::endl
            << "difference: " << (actual_contact_or_normal - expected_contact_or_normal).norm() << std::endl
            << "tolerance: " << tol << std::endl;
}

template <typename Shape1, typename Shape2>
void printComparisonError(const std::string& comparison_type,
                          const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
                          const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
                          GJKSolverType solver_type,
                          typename Shape1::S expected_depth,
                          typename Shape1::S actual_depth,
                          typename Shape1::S tol)
{
  std::cout << "Disagreement between " << comparison_type
            << " and expected_" << comparison_type << " for "
            << test::getNodeTypeName(s1.getNodeType()) << " and "
            << test::getNodeTypeName(s2.getNodeType()) << " with '"
            << test::getGJKSolverName(solver_type) << "' solver." << std::endl
            << "tf1.linear: \n" << tf1.linear() << std::endl
            << "tf1.translation: " << tf1.translation().transpose() << std::endl
            << "tf2.linear: \n" << tf2.linear() << std::endl
            << "tf2.translation: " << tf2.translation().transpose() << std::endl
            << "expected_depth: " << expected_depth << std::endl
            << "actual_depth  : " << actual_depth << std::endl
            << "difference: " << std::abs(actual_depth - expected_depth) << std::endl
            << "tolerance: " << tol << std::endl;
}

template <typename Shape1, typename Shape2>
bool checkContactPointds(const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
                        const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
                        GJKSolverType solver_type,
                        const ContactPoint<typename Shape1::S>& expected, const ContactPoint<typename Shape1::S>& actual,
                        bool check_position = false,
                        bool check_depth = false,
                        bool check_normal = false,
                        bool check_opposite_normal = false,
                        typename Shape1::S tol = 1e-9)
{
  FCL_UNUSED(s1);
  FCL_UNUSED(tf1);
  FCL_UNUSED(s2);
  FCL_UNUSED(tf2);
  FCL_UNUSED(solver_type);

  if (check_position)
  {
    bool contact_equal = actual.pos.isApprox(expected.pos, tol);
    if (!contact_equal)
      return false;
  }

  if (check_depth)
  {
    bool depth_equal = std::abs(actual.penetration_depth - expected.penetration_depth) < tol;
    if (!depth_equal)
      return false;
  }

  if (check_normal)
  {
    bool normal_equal = actual.normal.isApprox(expected.normal, tol);

    if (!normal_equal && check_opposite_normal)
      normal_equal = actual.normal.isApprox(-expected.normal, tol);

    if (!normal_equal)
      return false;
  }

  return true;
}

template <typename Shape1, typename Shape2>
bool inspectContactPointds(const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
                          const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
                          GJKSolverType solver_type,
                          const std::vector<ContactPoint<typename Shape1::S>>& expected_contacts,
                          const std::vector<ContactPoint<typename Shape1::S>>& actual_contacts,
                          bool check_position = false,
                          bool check_depth = false,
                          bool check_normal = false,
                          bool check_opposite_normal = false,
                          typename Shape1::S tol = 1e-9)
{
  using S = typename Shape1::S;

  // Check number of contact points
  bool sameNumContacts = (actual_contacts.size() == expected_contacts.size());
  EXPECT_TRUE(sameNumContacts);
  if (!sameNumContacts)
  {
    std::cout << "\n"
              << "===== [ geometric shape collision test failure report ] ======\n"
              << "\n"
              << "Solver type: " << test::getGJKSolverName(solver_type) << "\n"
              << "\n"
              << "[ Shape 1 ]\n"
              << "Shape type     : " << test::getNodeTypeName(s1.getNodeType()) << "\n"
              << "tf1.linear     : \n" << tf1.linear() << "\n"
              << "tf1.translation: " << tf1.translation().transpose() << "\n"
              << "\n"
              << "[ Shape 2 ]\n"
              << "Shape type     : " << test::getNodeTypeName(s2.getNodeType()) << "\n"
              << "tf2.linear     : \n" << tf2.linear() << "\n"
              << "tf2.translation: " << tf2.translation().transpose() << "\n"
              << "\n"
              << "The numbers of expected contacts '"
              << expected_contacts.size()
              << "' and the number of actual contacts '"
              << actual_contacts.size()
              << "' are not equal.\n"
              << "\n";
    return false;
  }

  // Check if actual contacts and expected contacts are matched
  const size_t numContacts = actual_contacts.size();

  std::vector<int> index_to_actual_contacts(numContacts, -1);
  std::vector<int> index_to_expected_contacts(numContacts, -1);

  bool foundAll = true;
  for (size_t i = 0; i < numContacts; ++i)
  {
    const ContactPoint<S>& expected = expected_contacts[i];

    // Check if expected contact is in the list of actual contacts
    for (size_t j = 0; j < numContacts; ++j)
    {
      if (index_to_expected_contacts[j] != -1)
        continue;

      const ContactPoint<S>& actual = actual_contacts[j];

      bool found = checkContactPointds(
            s1, tf1, s2, tf2, solver_type,
            expected, actual,
            check_position,
            check_depth,
            check_normal, check_opposite_normal,
            tol);

      if (found)
      {
        index_to_actual_contacts[i] = j;
        index_to_expected_contacts[j] = i;
        break;
      }
    }

    if (index_to_actual_contacts[i] == -1)
      foundAll = false;
  }

  if (!foundAll)
  {
    std::cout << "\n"
              << "===== [ geometric shape collision test failure report ] ======\n"
              << "\n"
              << "Solver type: " << test::getGJKSolverName(solver_type) << "\n"
              << "\n"
              << "[ Shape 1 ]\n"
              << "Shape type     : " << test::getNodeTypeName(s1.getNodeType()) << "\n"
              << "tf1.linear     : \n" << tf1.linear() << "\n"
              << "tf1.translation: " << tf1.translation().transpose() << "\n"
              << "\n"
              << "[ Shape 2 ]\n"
              << "Shape type     : " << test::getNodeTypeName(s2.getNodeType()) << "\n"
              << "tf2.linear     : \n" << tf2.linear() << "\n"
              << "tf2.translation: " << tf2.translation().transpose() << "\n"
              << "\n"
              << "[ Expected Contacts: " << numContacts << " ]\n";
    for (size_t i = 0; i < numContacts; ++i)
    {
      const ContactPoint<S>& expected = expected_contacts[i];

      std::cout << "(" << i << ") pos: " << expected.pos.transpose() << ", "
                << "normal: " << expected.normal.transpose() << ", "
                << "depth: " << expected.penetration_depth
                << " ---- ";
      if (index_to_actual_contacts[i] != -1)
        std::cout << "found, actual (" << index_to_actual_contacts[i] << ")\n";
      else
        std::cout << "not found!\n";
    }
    std::cout << "\n"
              << "[ Actual Contacts: " << numContacts << " ]\n";
    for (size_t i = 0; i < numContacts; ++i)
    {
      const ContactPoint<S>& actual = actual_contacts[i];

      std::cout << "(" << i << ") pos: " << actual.pos.transpose() << ", "
                << "normal: " << actual.normal.transpose() << ", "
                << "depth: " << actual.penetration_depth
                << " ---- ";
      if (index_to_expected_contacts[i] != -1)
        std::cout << "found, expected (" << index_to_expected_contacts[i] << ")\n";
      else
        std::cout << "not found!\n";
    }

    std::cout << "\n";
  }

  return foundAll;
}

template <typename S>
void getContactPointdsFromResult(std::vector<ContactPoint<S>>& contacts, const CollisionResult<S>& result)
{
  const size_t numContacts = result.numContacts();
  contacts.resize(numContacts);

  for (size_t i = 0; i < numContacts; ++i)
  {
    const auto& cnt = result.getContact(i);

    contacts[i].pos = cnt.pos;
    contacts[i].normal = cnt.normal;
    contacts[i].penetration_depth = cnt.penetration_depth;
  }
}

template <typename Shape1, typename Shape2>
void testShapeIntersection(
    const Shape1& s1, const Transform3<typename Shape1::S>& tf1,
    const Shape2& s2, const Transform3<typename Shape1::S>& tf2,
    GJKSolverType solver_type,
    bool expected_res,
    const std::vector<ContactPoint<typename Shape1::S>>& expected_contacts = std::vector<ContactPoint<typename Shape1::S>>(),
    bool check_position = true,
    bool check_depth = true,
    bool check_normal = true,
    bool check_opposite_normal = false,
    typename Shape1::S tol = 1e-9)
{
  using S = typename Shape1::S;

  CollisionRequest<S> request;
  request.gjk_solver_type = solver_type;
  request.num_max_contacts = std::numeric_limits<size_t>::max();
  CollisionResult<S> result;

  std::vector<ContactPoint<S>> actual_contacts;

  bool res;

  // Part A: Check collisions using shapeIntersect()

  // Check only whether they are colliding or not.
  if (solver_type == GST_LIBCCD)
  {
    res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, nullptr);
  }
  else if (solver_type == GST_INDEP)
  {
    res = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, nullptr);
  }
  else
  {
    std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
    return;
  }
  EXPECT_EQ(expected_res, res);

  // Check contact information as well
  if (solver_type == GST_LIBCCD)
  {
    res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts);
  }
  else if (solver_type == GST_INDEP)
  {
    res = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, &actual_contacts);
  }
  else
  {
    std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
    return;
  }
  EXPECT_EQ(expected_res, res);
  if (expected_res)
  {
    EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type,
                                     expected_contacts, actual_contacts,
                                     check_position,
                                     check_depth,
                                     check_normal, check_opposite_normal,
                                     tol));
  }

  // Part B: Check collisions using collide()

  // Check only whether they are colliding or not.
  request.enable_contact = false;
  result.clear();
  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
  EXPECT_EQ(expected_res, res);

  // Check contact information as well
  request.enable_contact = true;
  result.clear();
  res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
  EXPECT_EQ(expected_res, res);
  if (expected_res)
  {
    getContactPointdsFromResult(actual_contacts, result);
    EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type,
                                     expected_contacts, actual_contacts,
                                     check_position,
                                     check_depth,
                                     check_normal, check_opposite_normal,
                                     tol));
  }
}

// Shape intersection test coverage (libccd)
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box        |  O  |   O    |           |         |      |          |   O   |      O     |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |           |         |      |          |   O   |      O     |    O     |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid  |/////|////////|     O     |         |      |          |   O   |      O     |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|///////////|         |      |          |   O   |      O     |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|///////////|/////////|  O   |    O     |   O   |      O     |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|///////////|/////////|//////|    O     |   O   |      O     |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

template <typename S>
void test_shapeIntersection_spheresphere()
{
  Sphere<S> s1(20);
  Sphere<S> s2(10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  contacts[0].pos << 20, 0, 0;
  contacts[0].penetration_depth = 0.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0;
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[0].pos = transform * Vector3<S>(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0);
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
  contacts[0].pos.setZero();
  contacts[0].penetration_depth = 20.0 + 10.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
  contacts[0].pos = transform * Vector3<S>::Zero();
  contacts[0].penetration_depth = 20.0 + 10.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << -1, 0, 0;
  contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0;
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  contacts[0].pos = transform * Vector3<S>(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0);
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.0, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << -1, 0, 0;
  contacts[0].pos << -20, 0, 0;
  contacts[0].penetration_depth = 0.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheresphere)
{
//  test_shapeIntersection_spheresphere<float>();
  test_shapeIntersection_spheresphere<double>();
}

template <typename S>
bool compareContactPointds1(const Vector3<S>& c1,const Vector3<S>& c2)
{
  return c1[2] < c2[2];
} // Ascending order

template <typename S>
bool compareContactPointds2(const ContactPoint<S>& cp1,const ContactPoint<S>& cp2)
{
  return cp1.pos[2] < cp2.pos[2];
} // Ascending order

// Simple aligned boxes intersecting each other
//
//        s2  ┌───┐
//            │   │
//  ┏━━━━━━━━━┿━━━┿━━━━━━━━━┓┄┄┄┄┄ z = 0
//  ┃         │┄┄┄│┄┄┄┄┄┄┄┄┄┃┄┄┄ Reported contact depth
//  ┃         └───┘         ┃
//  ┃                       ┃
//  ┃                       ┃
//  ┃    s1                 ┃
//  ┃                       ┃
//  ┃                       ┃
//  ┃                       ┃
//  ┗━━━━━━━━━━━━━━━━━━━━━━━┛
//
//  s1 is a cube, 100 units to a side. Shifted downward 50 units so that it's
//     top face is at z = 0.
//  s2 is a box with dimensions 10, 20, 30 in the x-, y-, and z-directions,
//     respectively. It is centered on the origin and oriented according to the
//     provided rotation matrix `R`.
//  The total penetration depth is the |z_max| where z_max is the z-position of
//  the most deeply penetrating vertex on s2. The contact position will be
//  located at -z_max / 2 (with the assumption that all penetration happens
//  *below* the z = 0 axis.
template <typename Derived>
void testBoxBoxContactPointds(const Eigen::MatrixBase<Derived>& R)
{
  using S = typename Derived::RealScalar;

  Box<S> s1(100, 100, 100);
  Box<S> s2(10, 20, 30);

  // Vertices of s2
  std::vector<Vector3<S>> vertices(8);
  vertices[0] <<  1,  1,  1;
  vertices[1] <<  1,  1, -1;
  vertices[2] <<  1, -1,  1;
  vertices[3] <<  1, -1, -1;
  vertices[4] << -1,  1,  1;
  vertices[5] << -1,  1, -1;
  vertices[6] << -1, -1,  1;
  vertices[7] << -1, -1, -1;

  for (int i = 0; i < 8; ++i)
  {
    vertices[i][0] *= 0.5 * s2.side[0];
    vertices[i][1] *= 0.5 * s2.side[1];
    vertices[i][2] *= 0.5 * s2.side[2];
  }

  Transform3<S> tf1 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -50)));
  Transform3<S> tf2 = Transform3<S>(R);

  std::vector<ContactPoint<S>> contacts;

  // Make sure the two boxes are colliding
  bool res = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &contacts);
  EXPECT_TRUE(res);

  // Compute global vertices
  for (int i = 0; i < 8; ++i)
    vertices[i] = tf2 * vertices[i];

  // Sort the vertices so that the lowest vertex along z-axis comes first
  std::sort(vertices.begin(), vertices.end(), compareContactPointds1<S>);
  std::sort(contacts.begin(), contacts.end(), compareContactPointds2<S>);

  // The lowest n vertex along z-axis should be the contact point
  size_t numContacts = contacts.size();
  numContacts = std::min(static_cast<size_t>(1), numContacts);
  // TODO: BoxBox algorithm seems not able to find all the colliding vertices.
  // We just check the deepest one as workaround.
  for (size_t i = 0; i < numContacts; ++i)
  {
    // The reported contact position will lie mid-way between the deepest
    // penetrating vertex on s2 and the z = 0 plane.
    Vector3<S> contact_pos(vertices[i]);
    contact_pos(2) /= 2;
    EXPECT_TRUE(contact_pos.isApprox(contacts[i].pos))
              << "\n\tExpected: " << contact_pos
              << "\n\tFound:    " << contacts[i].pos;
    EXPECT_TRUE(Vector3<S>(0, 0, 1).isApprox(contacts[i].normal));
  }
}

template <typename S>
void test_shapeIntersection_boxbox()
{
  Box<S> s1(20, 40, 50);
  Box<S> s2(10, 10, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  Quaternion<S> q(AngleAxis<S>((S)3.140 / 6, Vector3<S>(0, 0, 1)));

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
  contacts.resize(4);
  contacts[0].normal << 1, 0, 0;
  contacts[1].normal << 1, 0, 0;
  contacts[2].normal << 1, 0, 0;
  contacts[3].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
  contacts.resize(4);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15, 0, 0)));
  contacts.resize(4);
  contacts[0].normal = Vector3<S>(1, 0, 0);
  contacts[1].normal = Vector3<S>(1, 0, 0);
  contacts[2].normal = Vector3<S>(1, 0, 0);
  contacts[3].normal = Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(q);
  contacts.resize(4);
  contacts[0].normal = Vector3<S>(1, 0, 0);
  contacts[1].normal = Vector3<S>(1, 0, 0);
  contacts[2].normal = Vector3<S>(1, 0, 0);
  contacts[3].normal = Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(q);
  contacts.resize(4);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  // Disabled for particular configurations: macOS + release + double (see #202)
#if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
  uint32 numTests = 1e+2;
  for (uint32 i = 0; i < numTests; ++i)
  {
    Transform3<S> tf;
    test::generateRandomTransform(extents<S>(), tf);
    testBoxBoxContactPointds(tf.linear());
  }
#endif
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_boxbox)
{
//  test_shapeIntersection_boxbox<float>();
  test_shapeIntersection_boxbox<double>();
}

template <typename S>
void test_shapeIntersection_spherebox()
{
  // A collection of bool constants to make reading lists of bool easier.
  const bool collides = true;
  const bool check_position = true;
  const bool check_depth = true;
  const bool check_normal = true;
  const bool check_opposite_normal = false;

  Sphere<S> s1(20);
  Box<S> s2(5, 5, 5);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // NOTE: The centers of the box and sphere are coincident. The documented
  // convention is that the normal is aligned with the smallest dimension of
  // the box, pointing in the negative direction of that axis. *This* test is
  // the driving basis for that definition.
  contacts.resize(1);
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
                        !check_position, !check_depth, check_normal);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
                        !check_position, !check_depth, !check_normal);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
  // As documented in sphere_box.h, touching is considered collision, so this
  // should produce a collision.
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
                        !check_position, !check_depth, !check_normal);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.501, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, !collides);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
                        !check_position, !check_depth, check_normal);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, collides, contacts,
                        !check_position, !check_depth, check_normal,
                        !check_opposite_normal, 1e-4);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherebox)
{
//  test_shapeIntersection_spherebox<float>();
  test_shapeIntersection_spherebox<double>();
}

template <typename S>
void test_shapeIntersection_spherecapsule()
{
  Sphere<S> s1(20);
  Capsule<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  //tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25 - 1e-6, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spherecapsule)
{
//  test_shapeIntersection_spherecapsule<float>();
  test_shapeIntersection_spherecapsule<double>();
}

template <typename S>
void test_shapeIntersection_cylindercylinder()
{
  Cylinder<S> s1(5, 10);
  Cylinder<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  // Disabled for particular configurations: macOS + release + double (see #202)
#if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
#endif

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercylinder)
{
//  test_shapeIntersection_cylindercylinder<float>();
  test_shapeIntersection_cylindercylinder<double>();
}

template <typename S>
void test_shapeIntersection_conecone()
{
  Cone<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 5e-5);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.001, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-5);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_conecone)
{
//  test_shapeIntersection_conecone<float>();
  test_shapeIntersection_conecone<double>();
}

template <typename S>
void test_shapeIntersection_cylindercone()
{
  Cylinder<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.061);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 0.46);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, true, false, 1e-4);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.01)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_cylindercone)
{
//  test_shapeIntersection_cylindercone<float>();
  test_shapeIntersection_cylindercone<double>();
}

template <typename S>
void test_shapeIntersection_ellipsoidellipsoid()
{
  Ellipsoid<S> s1(20, 40, 50);
  Ellipsoid<S> s2(10, 10, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);
  Transform3<S> identity;

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_ellipsoidellipsoid)
{
//  test_shapeIntersection_ellipsoidellipsoid<float>();
  test_shapeIntersection_ellipsoidellipsoid<double>();
}

template <typename S>
void test_shapeIntersection_spheretriangle()
{
  Sphere<S> s(10);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  Vector3<S> normal;
  bool res;

  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);


  t[0] << 30, 0, 0;
  t[1] << 9.9, -20, 0;
  t[2] << 9.9, 20, 0;
  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  res =  solver1<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_spheretriangle)
{
//  test_shapeIntersection_spheretriangle<float>();
  test_shapeIntersection_spheretriangle<double>();
}

template <typename S>
void test_shapeIntersection_halfspacetriangle()
{
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  // Vector3<S> point;
  // S depth;
  Vector3<S> normal;
  bool res;

  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);


  t[0] << 20, 0, 0;
  t[1] << 0, -20, 0;
  t[2] << 0, 20, 0;
  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  // Disabled for particular configurations: macOS + release + double (see #202)
#if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);
#endif

  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  // Disabled for particular configurations: macOS + release + double (see #202)
#if !defined(FCL_OS_MACOS) || !defined(NDEBUG)
  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
#endif
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacetriangle)
{
//  test_shapeIntersection_halfspacetriangle<float>();
  test_shapeIntersection_halfspacetriangle<double>();
}

template <typename S>
void test_shapeIntersection_planetriangle()
{
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  // Vector3<S> point;
  // S depth;
  Vector3<S> normal;
  bool res;

  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);


  t[0] << 20, 0, 0;
  t[1] << -0.1, -20, 0;
  t[2] << -0.1, 20, 0;
  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planetriangle)
{
//  test_shapeIntersection_planetriangle<float>();
  test_shapeIntersection_planetriangle<double>();
}

template <typename S>
void test_shapeIntersection_halfspacesphere()
{
  Sphere<S> s(10);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -5, 0, 0;
  contacts[0].penetration_depth = 10;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
  contacts[0].penetration_depth = 10;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 15;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 15;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -7.5, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-7.5, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.05, 0, 0;
  contacts[0].penetration_depth = 20.1;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
  contacts[0].penetration_depth = 20.1;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacesphere)
{
//  test_shapeIntersection_halfspacesphere<float>();
  test_shapeIntersection_halfspacesphere<double>();
}

template <typename S>
void test_shapeIntersection_planesphere()
{
  Sphere<S> s(10);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos.setZero();
  contacts[0].penetration_depth = 10;
  contacts[0].normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 10;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 5, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(5, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -5, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-5, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planesphere)
{
//  test_shapeIntersection_planesphere<float>();
  test_shapeIntersection_planesphere<double>();
}

template <typename S>
void test_shapeIntersection_halfspacebox()
{
  Box<S> s(5, 10, 20);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -0.625, 0, 0;
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-0.625, 0, 0);
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.875, 0, 0;
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.005, 0, 0;
  contacts[0].penetration_depth = 5.01;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
  contacts[0].penetration_depth = 5.01;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>(transform.linear());
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacebox)
{
//  test_shapeIntersection_halfspacebox<float>();
  test_shapeIntersection_halfspacebox<double>();
}

template <typename S>
void test_shapeIntersection_planebox()
{
  Box<S> s(5, 10, 20);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 1.25, 0, 0;
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, 0;
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
  contacts[0].penetration_depth = 1.25;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.51, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>(transform.linear());
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, false, false, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planebox)
{
//  test_shapeIntersection_planebox<float>();
  test_shapeIntersection_planebox<double>();
}

template <typename S>
void test_shapeIntersection_halfspaceellipsoid()
{
  Ellipsoid<S> s(5, 10, 20);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 5.0;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 5.0;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.875, 0, 0;
  contacts[0].penetration_depth = 6.25;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.875, 0, 0);
  contacts[0].penetration_depth = 6.25;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -3.125, 0, 0;
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-3.125, 0, 0);
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.005, 0, 0;
  contacts[0].penetration_depth = 10.01;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.005, 0, 0);
  contacts[0].penetration_depth = 10.01;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, -5.0, 0;
  contacts[0].penetration_depth = 10.0;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -5.0, 0);
  contacts[0].penetration_depth = 10.0;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -4.375, 0;
  contacts[0].penetration_depth = 11.25;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -4.375, 0);
  contacts[0].penetration_depth = 11.25;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -5.625, 0;
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -5.625, 0);
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 0.005, 0;
  contacts[0].penetration_depth = 20.01;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0.005, 0);
  contacts[0].penetration_depth = 20.01;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, -10.0;
  contacts[0].penetration_depth = 20.0;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -10.0);
  contacts[0].penetration_depth = 20.0;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -9.375;
  contacts[0].penetration_depth = 21.25;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -9.375);
  contacts[0].penetration_depth = 21.25;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -10.625;
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -10.625);
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0.005;
  contacts[0].penetration_depth = 40.01;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0.005);
  contacts[0].penetration_depth = 40.01;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspaceellipsoid)
{
//  test_shapeIntersection_halfspaceellipsoid<float>();
  test_shapeIntersection_halfspaceellipsoid<double>();
}

template <typename S>
void test_shapeIntersection_planeellipsoid()
{
  Ellipsoid<S> s(5, 10, 20);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5.0;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5.0;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 1.25, 0, 0;
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(1.25, 0, 0);
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, 0;
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-1.25, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
  contacts[0].penetration_depth = 3.75;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.01, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0.0, 0;
  contacts[0].penetration_depth = 10.0;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 10.0;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 1.25, 0;
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal << 0, 1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 1.25, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 1.25, 0);
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -1.25, 0;
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -1.25, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
  contacts[0].penetration_depth = 8.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -10.01, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 20.0;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 20.0;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 1.25;
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 1.25)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 1.25);
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -1.25;
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -1.25)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
  contacts[0].penetration_depth = 18.75;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -20.01)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planeellipsoid)
{
//  test_shapeIntersection_planeellipsoid<float>();
  test_shapeIntersection_planeellipsoid<double>();
}

template <typename S>
void test_shapeIntersection_halfspacecapsule()
{
  Capsule<S> s(5, 10);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, 0;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -3.75, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.05, 0, 0;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -1.25, 0;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -3.75, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 0.05, 0;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, -5;
  contacts[0].penetration_depth = 10;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -5);
  contacts[0].penetration_depth = 10;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -3.75;
  contacts[0].penetration_depth = 12.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
  contacts[0].penetration_depth = 12.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -6.25;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -6.25);
  contacts[0].penetration_depth  = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0.05;
  contacts[0].penetration_depth = 20.1;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
  contacts[0].penetration_depth = 20.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecapsule)
{
//  test_shapeIntersection_halfspacecapsule<float>();
  test_shapeIntersection_halfspacecapsule<double>();
}

template <typename S>
void test_shapeIntersection_planecapsule()
{
  Capsule<S> s(5, 10);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 2.5, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 1, 0;  // (0, 1, 0) or (0, -1, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);  // (0, 1, 0) or (0, -1, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 2.5, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 10;
  contacts[0].normal << 0, 0, 1;  // (0, 0, 1) or (0, 0, -1)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 10;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);  // (0, 0, 1) or (0, 0, -1)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 2.5;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -2.5;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecapsule)
{
//  test_shapeIntersection_planecapsule<float>();
  test_shapeIntersection_planecapsule<double>();
}

template <typename S>
void test_shapeIntersection_halfspacecylinder()
{
  Cylinder<S> s(5, 10);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, 0;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, 0);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -3.75, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-3.75, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.05, 0, 0;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.05, 0, 0);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -1.25, 0;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -1.25, 0);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -3.75, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -3.75, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 0.05, 0;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0.05, 0);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, -2.5;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -1.25;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -3.75;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0.05;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecylinder)
{
//  test_shapeIntersection_halfspacecylinder<float>();
  test_shapeIntersection_halfspacecylinder<double>();
}

template <typename S>
void test_shapeIntersection_planecylinder()
{
  Cylinder<S> s(5, 10);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 2.5, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(2.5, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 1, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 2.5, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 2.5, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, 0;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, 0);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 0, 1;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecylinder)
{
//  test_shapeIntersection_planecylinder<float>();
  test_shapeIntersection_planecylinder<double>();
}

template <typename S>
void test_shapeIntersection_halfspacecone()
{
  Cone<S> s(5, 10);
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, -5;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -5);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -1.25, 0, -5;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-1.25, 0, -5);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -3.75, 0, -5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-3.75, 0, -5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 0.05, 0, -5;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0.05, 0, -5);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, -5;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, -5);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -1.25, -5;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -1.25, -5);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -3.75, -5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -3.75, -5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 0.05, -5;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0.05, -5);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Halfspace<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, -2.5;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -1.25;
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -1.25);
  contacts[0].penetration_depth = 7.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -3.75;
  contacts[0].penetration_depth= 2.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -3.75);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0.05;
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 5.1)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0.05);
  contacts[0].penetration_depth = 10.1;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -5.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_halfspacecone)
{
//  test_shapeIntersection_halfspacecone<float>();
  test_shapeIntersection_halfspacecone<double>();
}

template <typename S>
void test_shapeIntersection_planecone()
{
  Cone<S> s(5, 10);
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 1, 0, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << 2.5, 0, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(2.5, 0, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos << -2.5, 0, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << -1, 0, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-2.5, 0, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(-2.5, 0, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-5.1, 0, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 1, 0), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 1, 0;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, 2.5, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 2.5, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos << 0, -2.5, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, -1, 0;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -2.5, 0)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, -2.5, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, -1, 0);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, -5.1, 0)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);




  hs = Plane<S>(Vector3<S>(0, 0, 1), 0);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].pos << 0, 0, 0;
  contacts[0].penetration_depth = 5;
  contacts[0].normal << 0, 0, 1;  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 0);
  contacts[0].penetration_depth = 5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);  // (1, 0, 0) or (-1, 0, 0)
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts, true, true, true, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, 2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, 2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos << 0, 0, -2.5;
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal << 0, 0, -1;
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -2.5)));
  contacts.resize(1);
  contacts[0].pos = transform * Vector3<S>(0, 0, -2.5);
  contacts[0].penetration_depth = 2.5;
  contacts[0].normal = transform.linear() * Vector3<S>(0, 0, -1);
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, -10.1)));
  testShapeIntersection(s, tf1, hs, tf2, GST_LIBCCD, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersection_planecone)
{
//  test_shapeIntersection_planecone<float>();
  test_shapeIntersection_planecone<double>();
}

// Shape distance test coverage (libccd)
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box        |  O  |   O    |           |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |           |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|///////////|/////////|  O   |    O     |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

template <typename S>
void test_shapeDistance_spheresphere()
{
  Sphere<S> s1(20);
  Sphere<S> s2(10);

  Transform3<S> transform = Transform3<S>::Identity();
  //test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist = -1;
  Vector3<S> closest_p1, closest_p2;
  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 40, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);


  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  // this is one problem: the precise is low sometimes
  EXPECT_TRUE(fabs(dist - 10) < 0.1);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.06);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.1);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.1);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_spheresphere)
{
//  test_shapeDistance_spheresphere<float>();
  test_shapeDistance_spheresphere<double>();
}

template <typename S>
void test_shapeDistance_boxbox()
{
  Box<S> s1(20, 40, 50);
  Box<S> s2(10, 10, 10);
  Vector3<S> closest_p1, closest_p2;

  Transform3<S> transform = Transform3<S>::Identity();
  //test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.2, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10.2) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20.1, 0, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 20.1, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s2, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 10.1, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 0.1 * 1.414) < 0.001);
  EXPECT_TRUE(res);


  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 5) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 5) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxbox)
{
//  test_shapeDistance_boxbox<float>();
  test_shapeDistance_boxbox<double>();
}

template <typename S>
void test_shapeDistance_boxsphere()
{
  Sphere<S> s1(20);
  Box<S> s2(5, 5, 5);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.05);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_boxsphere)
{
//  test_shapeDistance_boxsphere<float>();
  test_shapeDistance_boxsphere<double>();
}

template <typename S>
void test_shapeDistance_cylindercylinder()
{
  Cylinder<S> s1(5, 10);
  Cylinder<S> s2(5, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_cylindercylinder)
{
//  test_shapeDistance_cylindercylinder<float>();
  test_shapeDistance_cylindercylinder<double>();
}

template <typename S>
void test_shapeDistance_conecone()
{
  Cone<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 1);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 1);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecone)
{
//  test_shapeDistance_conecone<float>();
  test_shapeDistance_conecone<double>();
}

template <typename S>
void test_shapeDistance_conecylinder()
{
  Cylinder<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.02);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.01);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.1);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_conecylinder)
{
//  test_shapeDistance_conecylinder<float>();
  test_shapeDistance_conecylinder<double>();
}

template <typename S>
void test_shapeDistance_ellipsoidellipsoid()
{
  Ellipsoid<S> s1(20, 40, 50);
  Ellipsoid<S> s2(10, 10, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist = -1;
  Vector3<S> closest_p1, closest_p2;

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist, &closest_p1, &closest_p2);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);


  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver1<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistance_ellipsoidellipsoid)
{
//  test_shapeDistance_ellipsoidellipsoid<float>();
  test_shapeDistance_ellipsoidellipsoid<double>();
}

// Shape intersection test coverage (built-in GJK)
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box        |  O  |   O    |           |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |           |         |      |          |       |            |     O    |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|///////////|/////////|  O   |    O     |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

template <typename S>
void test_shapeIntersectionGJK_spheresphere()
{
  Sphere<S> s1(20);
  Sphere<S> s2(10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  contacts[0].pos << 20, 0, 0;
  contacts[0].penetration_depth = 0.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  contacts[0].pos << 20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0;
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[0].pos = transform * Vector3<S>(20.0 - 0.1 * 20.0/(20.0 + 10.0), 0, 0);
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  contacts[0].normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
  contacts[0].pos.setZero();
  contacts[0].penetration_depth = 20.0 + 10.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  contacts[0].normal.setZero();  // If the centers of two sphere are at the same position, the normal is (0, 0, 0)
  contacts[0].pos = transform * Vector3<S>::Zero();
  contacts[0].penetration_depth = 20.0 + 10.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << -1, 0, 0;
  contacts[0].pos << -20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0;
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(-1, 0, 0);
  contacts[0].pos = transform * Vector3<S>(-20.0 + 0.1 * 20.0/(20.0 + 10.0), 0, 0);
  contacts[0].penetration_depth = 0.1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.0, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << -1, 0, 0;
  contacts[0].pos << -20, 0, 0;
  contacts[0].penetration_depth = 0.0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheresphere)
{
//  test_shapeIntersectionGJK_spheresphere<float>();
  test_shapeIntersectionGJK_spheresphere<double>();
}

template <typename S>
void test_shapeIntersectionGJK_boxbox()
{
  Box<S> s1(20, 40, 50);
  Box<S> s2(10, 10, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  Quaternion<S> q(AngleAxis<S>((S)3.140 / 6, Vector3<S>(0, 0, 1)));

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
  contacts.resize(4);
  contacts[0].normal << 1, 0, 0;
  contacts[1].normal << 1, 0, 0;
  contacts[2].normal << 1, 0, 0;
  contacts[3].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position. The current result is (1, 0, 0).
  contacts.resize(4);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(15, 0, 0)));
  contacts.resize(4);
  contacts[0].normal = Vector3<S>(1, 0, 0);
  contacts[1].normal = Vector3<S>(1, 0, 0);
  contacts[2].normal = Vector3<S>(1, 0, 0);
  contacts[3].normal = Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(15.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(q);
  contacts.resize(4);
  contacts[0].normal = Vector3<S>(1, 0, 0);
  contacts[1].normal = Vector3<S>(1, 0, 0);
  contacts[2].normal = Vector3<S>(1, 0, 0);
  contacts[3].normal = Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(q);
  contacts.resize(4);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[1].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[2].normal = transform.linear() * Vector3<S>(1, 0, 0);
  contacts[3].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_boxbox)
{
//  test_shapeIntersectionGJK_boxbox<float>();
  test_shapeIntersectionGJK_boxbox<double>();
}

template <typename S>
void test_shapeIntersectionGJK_spherebox()
{
  Sphere<S> s1(20);
  Box<S> s2(5, 5, 5);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  // TODO(SeanCurtis-TRI): This osculating contact is considered a collision
  // only for a relatively "loose" gjk tolerance. So, for this test to pass, we
  // set and restore the default gjk tolerances. We need to determine if this is
  // the correct/desired behavior and, if so, fix the defect that smaller
  // tolerances prevent this from reporting as a contact. NOTE: this is *not*
  // the same tolerance that is passed into the `testShapeIntersection` function
  // -- which isn't actually *used*.
  {
    detail::GJKSolver_indep<S>& solver = solver2<S>();
    const S old_gjk_tolerance = solver.gjk_tolerance;
    const S old_epa_tolerance = solver.epa_tolerance;

    // The historical tolerances for which this test passes.
    solver.gjk_tolerance = 1e-6;
    solver.epa_tolerance = 1e-6;

    testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false,
                          false, true, false,
                          1e-7 // built-in GJK solver requires larger tolerance than libccd
    );
    // TODO(SeanCurtis-TRI): If testShapeIntersection fails an *assert* this
    // code will not get fired off and the static solvers will not get reset.
    solver.gjk_tolerance = old_gjk_tolerance;
    solver.epa_tolerance = old_epa_tolerance;
  }

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.51, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-2);  // built-in GJK solver requires larger tolerance than libccd

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.4, 0, 0)));
  contacts.resize(1);
  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
  // built-in GJK solver returns incorrect normal.
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherebox)
{
//  test_shapeIntersectionGJK_spherebox<float>();
  test_shapeIntersectionGJK_spherebox<double>();
}

template <typename S>
void test_shapeIntersectionGJK_spherecapsule()
{
  Sphere<S> s1(20);
  Capsule<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(24.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(25, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(25.1, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spherecapsule)
{
//  test_shapeIntersectionGJK_spherecapsule<float>();
  test_shapeIntersectionGJK_spherecapsule<double>();
}

template <typename S>
void test_shapeIntersectionGJK_cylindercylinder()
{
  Cylinder<S> s1(5, 10);
  Cylinder<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 3e-1);  // built-in GJK solver requires larger tolerance than libccd

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
  // built-in GJK solver returns incorrect normal.

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercylinder)
{
//  test_shapeIntersectionGJK_cylindercylinder<float>();
  test_shapeIntersectionGJK_cylindercylinder<double>();
}

template <typename S>
void test_shapeIntersectionGJK_conecone()
{
  Cone<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  contacts[0].normal << 1, 0, 0;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 5.7e-1);  // built-in GJK solver requires larger tolerance than libccd

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
  // built-in GJK solver returns incorrect normal.

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  // contacts[0].normal = transform.linear() * Vector3<S>(0, 0, 1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
  // built-in GJK solver returns incorrect normal.
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_conecone)
{
//  test_shapeIntersectionGJK_conecone<float>();
  test_shapeIntersectionGJK_conecone<double>();
}

template <typename S>
void test_shapeIntersectionGJK_cylindercone()
{
  Cylinder<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  std::vector<ContactPoint<S>> contacts;
  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  // TODO: Need convention for normal when the centers of two objects are at same position.
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(9.9, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(10, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 9.9)));
  contacts.resize(1);
  // contacts[0].normal = transform.linear() * Vector3<S>(1, 0, 0);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);
  // built-in GJK solver returns incorrect normal.

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10)));
  contacts.resize(1);
  contacts[0].normal << 0, 0, 1;
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 10.1)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_cylindercone)
{
//  test_shapeIntersectionGJK_cylindercone<float>();
  test_shapeIntersectionGJK_cylindercone<double>();
}

template <typename S>
void test_shapeIntersectionGJK_ellipsoidellipsoid()
{
  Ellipsoid<S> s1(20, 40, 50);
  Ellipsoid<S> s2(10, 10, 10);

  Transform3<S> tf1;
  Transform3<S> tf2;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);
  Transform3<S> identity;

  std::vector<ContactPoint<S>> contacts;

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(30, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>::Identity();
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform;
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-29.99, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = Transform3<S>::Identity();
  tf2 = Transform3<S>(Translation3<S>(Vector3<S>(-30, 0, 0)));
  contacts.resize(1);
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, false);

  tf1 = transform;
  tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(-30.01, 0, 0)));
  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, false);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_ellipsoidellipsoid)
{
//  test_shapeIntersectionGJK_ellipsoidellipsoid<float>();
  test_shapeIntersectionGJK_ellipsoidellipsoid<double>();
}

template <typename S>
void test_shapeIntersectionGJK_spheretriangle()
{
  Sphere<S> s(10);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  // Vector3<S> point;
  // S depth;
  Vector3<S> normal;
  bool res;

  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  t[0] << 30, 0, 0;
  t[1] << 9.9, -20, 0;
  t[2] << 9.9, 20, 0;
  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeTriangleIntersect(s, Transform3<S>::Identity(), t[0], t[1], t[2], nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  res =  solver2<S>().shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_spheretriangle)
{
//  test_shapeIntersectionGJK_spheretriangle<float>();
  test_shapeIntersectionGJK_spheretriangle<double>();
}

template <typename S>
void test_shapeIntersectionGJK_halfspacetriangle()
{
  Halfspace<S> hs(Vector3<S>(1, 0, 0), 0);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  // Vector3<S> point;
  // S depth;
  Vector3<S> normal;
  bool res;

  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);


  t[0] << 20, 0, 0;
  t[1] << -0.1, -20, 0;
  t[2] << -0.1, 20, 0;
  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  res =  solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_halfspacetriangle)
{
//  test_shapeIntersectionGJK_halfspacetriangle<float>();
  test_shapeIntersectionGJK_halfspacetriangle<double>();
}

template <typename S>
void test_shapeIntersectionGJK_planetriangle()
{
  Plane<S> hs(Vector3<S>(1, 0, 0), 0);
  Vector3<S> t[3];
  t[0] << 20, 0, 0;
  t[1] << -20, 0, 0;
  t[2] << 0, 20, 0;

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  // Vector3<S> point;
  // S depth;
  Vector3<S> normal;
  bool res;

  res = solver1<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver1<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);


  t[0] << 20, 0, 0;
  t[1] << -0.1, -20, 0;
  t[2] << -0.1, 20, 0;
  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res =  solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, nullptr);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeTriangleIntersect(hs, Transform3<S>::Identity(), t[0], t[1], t[2], Transform3<S>::Identity(), nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(Vector3<S>(1, 0, 0), 1e-9));

  res =  solver2<S>().shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, nullptr, nullptr, &normal);
  EXPECT_TRUE(res);
  EXPECT_TRUE(normal.isApprox(transform.linear() * Vector3<S>(1, 0, 0), 1e-9));
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeIntersectionGJK_planetriangle)
{
//  test_shapeIntersectionGJK_planetriangle<float>();
  test_shapeIntersectionGJK_planetriangle<double>();
}

// Shape distance test coverage (built-in GJK)
//
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// |            | box | sphere | ellipsoid | capsule | cone | cylinder | plane | half-space | triangle |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | box        |  O  |   O    |           |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | sphere     |/////|   O    |           |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | ellipsoid  |/////|////////|     O     |         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | capsule    |/////|////////|///////////|         |      |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cone       |/////|////////|///////////|/////////|  O   |          |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | cylinder   |/////|////////|///////////|/////////|//////|    O     |       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | plane      |/////|////////|///////////|/////////|//////|//////////|       |            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | half-space |/////|////////|///////////|/////////|//////|//////////|///////|            |          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+
// | triangle   |/////|////////|///////////|/////////|//////|//////////|///////|////////////|          |
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

template <typename S>
void test_shapeDistanceGJK_spheresphere()
{
  Sphere<S> s1(20);
  Sphere<S> s2(10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist = -1;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);


  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_spheresphere)
{
//  test_shapeDistanceGJK_spheresphere<float>();
  test_shapeDistanceGJK_spheresphere<double>();
}

template <typename S>
void test_shapeDistanceGJK_boxbox()
{
  Box<S> s1(20, 40, 50);
  Box<S> s2(10, 10, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(15.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 5) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(20, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 5) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxbox)
{
//  test_shapeDistanceGJK_boxbox<float>();
  test_shapeDistanceGJK_boxbox<double>();
}

template <typename S>
void test_shapeDistanceGJK_boxsphere()
{
  Sphere<S> s1(20);
  Box<S> s2(5, 5, 5);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(22.6, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.01);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 17.5) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_boxsphere)
{
//  test_shapeDistanceGJK_boxsphere<float>();
  test_shapeDistanceGJK_boxsphere<double>();
}

template <typename S>
void test_shapeDistanceGJK_cylindercylinder()
{
  Cylinder<S> s1(5, 10);
  Cylinder<S> s2(5, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_cylindercylinder)
{
//  test_shapeDistanceGJK_cylindercylinder<float>();
  test_shapeDistanceGJK_cylindercylinder<double>();
}

template <typename S>
void test_shapeDistanceGJK_conecone()
{
  Cone<S> s1(5, 10);
  Cone<S> s2(5, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(10.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(0, 0, 40))), &dist);
  EXPECT_TRUE(fabs(dist - 30) < 0.001);
  EXPECT_TRUE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_conecone)
{
//  test_shapeDistanceGJK_conecone<float>();
  test_shapeDistanceGJK_conecone<double>();
}

template <typename S>
void test_shapeDistanceGJK_ellipsoidellipsoid()
{
  Ellipsoid<S> s1(20, 40, 50);
  Ellipsoid<S> s2(10, 10, 10);

  Transform3<S> transform = Transform3<S>::Identity();
  test::generateRandomTransform(extents<S>(), transform);

  bool res;
  S dist = -1;

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>::Identity(), s2, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, Transform3<S>::Identity(), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform, s2, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(40, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 10) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(30.1, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(fabs(dist - 0.1) < 0.001);
  EXPECT_TRUE(res);

  res = solver2<S>().shapeDistance(s1, transform * Transform3<S>(Translation3<S>(Vector3<S>(29.9, 0, 0))), s2, transform, &dist);
  EXPECT_TRUE(dist < 0);
  EXPECT_FALSE(res);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, shapeDistanceGJK_ellipsoidellipsoid)
{
//  test_shapeDistanceGJK_ellipsoidellipsoid<float>();
  test_shapeDistanceGJK_ellipsoidellipsoid<double>();
}

template<typename Shape1, typename Shape2>
void testReversibleShapeIntersection(const Shape1& s1, const Shape2& s2, typename Shape2::S distance)
{
  using S = typename Shape2::S;

  Transform3<S> tf1(Translation3<S>(Vector3<S>(-0.5 * distance, 0.0, 0.0)));
  Transform3<S> tf2(Translation3<S>(Vector3<S>(+0.5 * distance, 0.0, 0.0)));

  std::vector<ContactPoint<S>> contactsA;
  std::vector<ContactPoint<S>> contactsB;

  bool resA;
  bool resB;

  const double tol = 1e-6;

  resA = solver1<S>().shapeIntersect(s1, tf1, s2, tf2, &contactsA);
  resB = solver1<S>().shapeIntersect(s2, tf2, s1, tf1, &contactsB);

  // normal should be opposite
  for (size_t i = 0; i < contactsB.size(); ++i)
    contactsB[i].normal = -contactsB[i].normal;

  EXPECT_TRUE(resA);
  EXPECT_TRUE(resB);
  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_LIBCCD,
                                   contactsA, contactsB,
                                   true, true, true, false, tol));

  resA = solver2<S>().shapeIntersect(s1, tf1, s2, tf2, &contactsA);
  resB = solver2<S>().shapeIntersect(s2, tf2, s1, tf1, &contactsB);

  // normal should be opposite
  for (size_t i = 0; i < contactsB.size(); ++i)
    contactsB[i].normal = -contactsB[i].normal;

  EXPECT_TRUE(resA);
  EXPECT_TRUE(resB);
  EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, GST_INDEP,
                                   contactsA, contactsB,
                                   true, true, true, false, tol));
}

template <typename S>
void test_reversibleShapeIntersection_allshapes()
{
  // This test check whether a shape intersection algorithm is called for the
  // reverse case as well. For example, if FCL has sphere-capsule intersection
  // algorithm, then this algorithm should be called for capsule-sphere case.

  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
  Box<S> box(10, 10, 10);
  Sphere<S> sphere(5);
  Ellipsoid<S> ellipsoid(5, 5, 5);
  Capsule<S> capsule(5, 10);
  Cone<S> cone(5, 10);
  Cylinder<S> cylinder(5, 10);
  Plane<S> plane(Vector3<S>::Zero(), 0.0);
  Halfspace<S> halfspace(Vector3<S>::Zero(), 0.0);

  // Use sufficiently short distance so that all the primitive shapes can intersect
  S distance = 5.0;

  // If new shape intersection algorithm is added for two distinct primitive
  // shapes, uncomment associated lines. For example, box-sphere intersection
  // algorithm is added, then uncomment box-sphere.

//  testReversibleShapeIntersection(box, sphere, distance);
//  testReversibleShapeIntersection(box, ellipsoid, distance);
//  testReversibleShapeIntersection(box, capsule, distance);
//  testReversibleShapeIntersection(box, cone, distance);
//  testReversibleShapeIntersection(box, cylinder, distance);
  testReversibleShapeIntersection(box, plane, distance);
  testReversibleShapeIntersection(box, halfspace, distance);

//  testReversibleShapeIntersection(sphere, ellipsoid, distance);
  testReversibleShapeIntersection(sphere, capsule, distance);
//  testReversibleShapeIntersection(sphere, cone, distance);
//  testReversibleShapeIntersection(sphere, cylinder, distance);
  testReversibleShapeIntersection(sphere, plane, distance);
  testReversibleShapeIntersection(sphere, halfspace, distance);

//  testReversibleShapeIntersection(ellipsoid, capsule, distance);
//  testReversibleShapeIntersection(ellipsoid, cone, distance);
//  testReversibleShapeIntersection(ellipsoid, cylinder, distance);
//  testReversibleShapeIntersection(ellipsoid, plane, distance);
//  testReversibleShapeIntersection(ellipsoid, halfspace, distance);

//  testReversibleShapeIntersection(capsule, cone, distance);
//  testReversibleShapeIntersection(capsule, cylinder, distance);
  testReversibleShapeIntersection(capsule, plane, distance);
  testReversibleShapeIntersection(capsule, halfspace, distance);

//  testReversibleShapeIntersection(cone, cylinder, distance);
  testReversibleShapeIntersection(cone, plane, distance);
  testReversibleShapeIntersection(cone, halfspace, distance);

  testReversibleShapeIntersection(cylinder, plane, distance);
  testReversibleShapeIntersection(cylinder, halfspace, distance);

  testReversibleShapeIntersection(plane, halfspace, distance);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeIntersection_allshapes)
{
//  test_reversibleShapeIntersection_allshapes<float>();
  test_reversibleShapeIntersection_allshapes<double>();
}

template<typename Shape1, typename Shape2>
void testReversibleShapeDistance(const Shape1& s1, const Shape2& s2, typename Shape2::S distance)
{
  using S = typename Shape2::S;

  Transform3<S> tf1(Translation3<S>(Vector3<S>(-0.5 * distance, 0.0, 0.0)));
  Transform3<S> tf2(Translation3<S>(Vector3<S>(+0.5 * distance, 0.0, 0.0)));

  S distA;
  S distB;
  Vector3<S> p1A;
  Vector3<S> p1B;
  Vector3<S> p2A;
  Vector3<S> p2B;

  bool resA;
  bool resB;

  const double tol = 1e-6;

  resA = solver1<S>().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
  resB = solver1<S>().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);

  EXPECT_TRUE(resA);
  EXPECT_TRUE(resB);
  EXPECT_NEAR(distA, distB, tol);  // distances should be same
  EXPECT_TRUE(p1A.isApprox(p2B, tol));  // closest points should in reverse order
  EXPECT_TRUE(p2A.isApprox(p1B, tol));

  resA = solver2<S>().shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
  resB = solver2<S>().shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);

  EXPECT_TRUE(resA);
  EXPECT_TRUE(resB);
  EXPECT_NEAR(distA, distB, tol);
  EXPECT_TRUE(p1A.isApprox(p2B, tol));
  EXPECT_TRUE(p2A.isApprox(p1B, tol));
}

template <typename S>
void test_reversibleShapeDistance_allshapes()
{
  // This test check whether a shape distance algorithm is called for the
  // reverse case as well. For example, if FCL has sphere-capsule distance
  // algorithm, then this algorithm should be called for capsule-sphere case.

  // Prepare all kinds of primitive shapes (8) -- box, sphere, ellipsoid, capsule, cone, cylinder, plane, halfspace
  Box<S> box(10, 10, 10);
  Sphere<S> sphere(5);
  Ellipsoid<S> ellipsoid(5, 5, 5);
  Capsule<S> capsule(5, 10);
  Cone<S> cone(5, 10);
  Cylinder<S> cylinder(5, 10);
  Plane<S> plane(Vector3<S>::Zero(), 0.0);
  Halfspace<S> halfspace(Vector3<S>::Zero(), 0.0);

  // Use sufficiently long distance so that all the primitive shapes CANNOT intersect
  S distance = 15.0;

  // If new shape distance algorithm is added for two distinct primitive
  // shapes, uncomment associated lines. For example, box-sphere intersection
  // algorithm is added, then uncomment box-sphere.

//  testReversibleShapeDistance(box, sphere, distance);
//  testReversibleShapeDistance(box, ellipsoid, distance);
//  testReversibleShapeDistance(box, capsule, distance);
//  testReversibleShapeDistance(box, cone, distance);
//  testReversibleShapeDistance(box, cylinder, distance);
//  testReversibleShapeDistance(box, plane, distance);
//  testReversibleShapeDistance(box, halfspace, distance);

//  testReversibleShapeDistance(sphere, ellipsoid, distance);
  testReversibleShapeDistance(sphere, capsule, distance);
//  testReversibleShapeDistance(sphere, cone, distance);
//  testReversibleShapeDistance(sphere, cylinder, distance);
//  testReversibleShapeDistance(sphere, plane, distance);
//  testReversibleShapeDistance(sphere, halfspace, distance);

//  testReversibleShapeDistance(ellipsoid, capsule, distance);
//  testReversibleShapeDistance(ellipsoid, cone, distance);
//  testReversibleShapeDistance(ellipsoid, cylinder, distance);
//  testReversibleShapeDistance(ellipsoid, plane, distance);
//  testReversibleShapeDistance(ellipsoid, halfspace, distance);

//  testReversibleShapeDistance(capsule, cone, distance);
//  testReversibleShapeDistance(capsule, cylinder, distance);
//  testReversibleShapeDistance(capsule, plane, distance);
//  testReversibleShapeDistance(capsule, halfspace, distance);

//  testReversibleShapeDistance(cone, cylinder, distance);
//  testReversibleShapeDistance(cone, plane, distance);
//  testReversibleShapeDistance(cone, halfspace, distance);

//  testReversibleShapeDistance(cylinder, plane, distance);
//  testReversibleShapeDistance(cylinder, halfspace, distance);

//  testReversibleShapeDistance(plane, halfspace, distance);
}

GTEST_TEST(FCL_GEOMETRIC_SHAPES, reversibleShapeDistance_allshapes)
{
//  test_reversibleShapeDistance_allshapes<float>();
  test_reversibleShapeDistance_allshapes<double>();
}

//==============================================================================
int main(int argc, char* argv[])
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}