coal/internal/traversal_node_hfield_shape.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021-2024, INRIA.
 *  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.
 */
 
#ifndef COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
#define COAL_TRAVERSAL_NODE_HFIELD_SHAPE_H
 
 
#include "coal/collision_data.h"
#include "coal/shape/geometric_shapes.h"
#include "coal/narrowphase/narrowphase.h"
#include "coal/shape/geometric_shapes_utility.h"
#include "coal/internal/shape_shape_func.h"
#include "coal/internal/traversal_node_base.h"
#include "coal/internal/traversal.h"
#include "coal/internal/intersect.h"
#include "coal/hfield.h"
#include "coal/shape/convex.h"
 
namespace coal {
 
 
namespace details {
template <typename BV>
Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
                                               const HeightField<BV>& model) {
  const MatrixXs& heights = model.getHeights();
  const VecXs& x_grid = model.getXGrid();
  const VecXs& y_grid = model.getYGrid();
 
  const CoalScalar min_height = model.getMinHeight();
 
  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
  const Eigen::Block<const MatrixXs, 2, 2> cell =
      heights.block<2, 2>(node.y_id, node.x_id);
 
  assert(cell.maxCoeff() > min_height &&
         "max_height is lower than min_height");  // Check whether the geometry
                                                  // is degenerated
 
  std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
      Vec3s(x0, y0, min_height),
      Vec3s(x0, y1, min_height),
      Vec3s(x1, y1, min_height),
      Vec3s(x1, y0, min_height),
      Vec3s(x0, y0, cell(0, 0)),
      Vec3s(x0, y1, cell(1, 0)),
      Vec3s(x1, y1, cell(1, 1)),
      Vec3s(x1, y0, cell(0, 1)),
  }));
 
  std::shared_ptr<std::vector<Quadrilateral>> polygons(
      new std::vector<Quadrilateral>(6));
  (*polygons)[0].set(0, 3, 2, 1);  // x+ side
  (*polygons)[1].set(0, 1, 5, 4);  // y- side
  (*polygons)[2].set(1, 2, 6, 5);  // x- side
  (*polygons)[3].set(2, 3, 7, 6);  // y+ side
  (*polygons)[4].set(3, 0, 4, 7);  // z- side
  (*polygons)[5].set(4, 5, 6, 7);  // z+ side
 
  return Convex<Quadrilateral>(pts,  // points
                               8,    // num points
                               polygons,
                               6  // number of polygons
  );
}
 
enum class FaceOrientationConvexPart1 {
  BOTTOM = 0,
  TOP = 1,
  WEST = 2,
  SOUTH_EAST = 4,
  NORTH = 8,
};
 
enum class FaceOrientationConvexPart2 {
  BOTTOM = 0,
  TOP = 1,
  SOUTH = 2,
  NORTH_WEST = 4,
  EAST = 8,
};
 
template <typename BV>
void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
                          Convex<Triangle>& convex1, int& convex1_active_faces,
                          Convex<Triangle>& convex2,
                          int& convex2_active_faces) {
  const MatrixXs& heights = model.getHeights();
  const VecXs& x_grid = model.getXGrid();
  const VecXs& y_grid = model.getYGrid();
 
  const CoalScalar min_height = model.getMinHeight();
 
  const CoalScalar x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1],
                   y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1];
  const CoalScalar max_height = node.max_height;
  const Eigen::Block<const MatrixXs, 2, 2> cell =
      heights.block<2, 2>(node.y_id, node.x_id);
 
  const int contact_active_faces = node.contact_active_faces;
  convex1_active_faces = 0;
  convex2_active_faces = 0;
 
  typedef HFNodeBase::FaceOrientation FaceOrientation;
 
  if (contact_active_faces & FaceOrientation::TOP) {
    convex1_active_faces |= int(details::FaceOrientationConvexPart1::TOP);
    convex2_active_faces |= int(details::FaceOrientationConvexPart2::TOP);
  }
 
  if (contact_active_faces & FaceOrientation::BOTTOM) {
    convex1_active_faces |= int(details::FaceOrientationConvexPart1::BOTTOM);
    convex2_active_faces |= int(details::FaceOrientationConvexPart2::BOTTOM);
  }
 
  // Specific orientation for Convex1
  if (contact_active_faces & FaceOrientation::WEST) {
    convex1_active_faces |= int(details::FaceOrientationConvexPart1::WEST);
  }
 
  if (contact_active_faces & FaceOrientation::NORTH) {
    convex1_active_faces |= int(details::FaceOrientationConvexPart1::NORTH);
  }
 
  // Specific orientation for Convex2
  if (contact_active_faces & FaceOrientation::EAST) {
    convex2_active_faces |= int(details::FaceOrientationConvexPart2::EAST);
  }
 
  if (contact_active_faces & FaceOrientation::SOUTH) {
    convex2_active_faces |= int(details::FaceOrientationConvexPart2::SOUTH);
  }
 
  assert(max_height > min_height &&
         "max_height is lower than min_height");  // Check whether the geometry
                                                  // is degenerated
  COAL_UNUSED_VARIABLE(max_height);
 
  {
    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
        Vec3s(x0, y0, min_height),  // A
        Vec3s(x0, y1, min_height),  // B
        Vec3s(x1, y0, min_height),  // C
        Vec3s(x0, y0, cell(0, 0)),  // D
        Vec3s(x0, y1, cell(1, 0)),  // E
        Vec3s(x1, y0, cell(0, 1)),  // F
    }));
 
    std::shared_ptr<std::vector<Triangle>> triangles(
        new std::vector<Triangle>(8));
    (*triangles)[0].set(0, 2, 1);  // bottom
    (*triangles)[1].set(3, 4, 5);  // top
    (*triangles)[2].set(0, 1, 3);  // West 1
    (*triangles)[3].set(3, 1, 4);  // West 2
    (*triangles)[4].set(1, 2, 5);  // South-East 1
    (*triangles)[5].set(1, 5, 4);  // South-East 1
    (*triangles)[6].set(0, 5, 2);  // North 1
    (*triangles)[7].set(5, 0, 3);  // North 2
 
    convex1.set(pts,  // points
                6,    // num points
                triangles,
                8  // number of polygons
    );
  }
 
  {
    std::shared_ptr<std::vector<Vec3s>> pts(new std::vector<Vec3s>({
        Vec3s(x0, y1, min_height),  // A
        Vec3s(x1, y1, min_height),  // B
        Vec3s(x1, y0, min_height),  // C
        Vec3s(x0, y1, cell(1, 0)),  // D
        Vec3s(x1, y1, cell(1, 1)),  // E
        Vec3s(x1, y0, cell(0, 1)),  // F
    }));
 
    std::shared_ptr<std::vector<Triangle>> triangles(
        new std::vector<Triangle>(8));
    (*triangles)[0].set(2, 1, 0);  // bottom
    (*triangles)[1].set(3, 4, 5);  // top
    (*triangles)[2].set(0, 1, 3);  // South 1
    (*triangles)[3].set(3, 1, 4);  // South 2
    (*triangles)[4].set(0, 5, 2);  // North West 1
    (*triangles)[5].set(0, 3, 5);  // North West 2
    (*triangles)[6].set(1, 2, 5);  // East 1
    (*triangles)[7].set(4, 1, 2);  // East 2
 
    convex2.set(pts,  // points
                6,    // num points
                triangles,
                8  // number of polygons
    );
  }
}
 
inline Vec3s projectTriangle(const Vec3s& pointA, const Vec3s& pointB,
                             const Vec3s& pointC, const Vec3s& point) {
  const Project::ProjectResult result =
      Project::projectTriangle(pointA, pointB, pointC, point);
  Vec3s res = result.parameterization[0] * pointA +
              result.parameterization[1] * pointB +
              result.parameterization[2] * pointC;
 
  return res;
}
 
inline Vec3s projectTetrahedra(const Vec3s& pointA, const Vec3s& pointB,
                               const Vec3s& pointC, const Vec3s& pointD,
                               const Vec3s& point) {
  const Project::ProjectResult result =
      Project::projectTetrahedra(pointA, pointB, pointC, pointD, point);
  Vec3s res = result.parameterization[0] * pointA +
              result.parameterization[1] * pointB +
              result.parameterization[2] * pointC +
              result.parameterization[3] * pointD;
 
  return res;
}
 
inline Vec3s computeTriangleNormal(const Triangle& triangle,
                                   const std::vector<Vec3s>& points) {
  const Vec3s pointA = points[triangle[0]];
  const Vec3s pointB = points[triangle[1]];
  const Vec3s pointC = points[triangle[2]];
 
  const Vec3s normal = (pointB - pointA).cross(pointC - pointA).normalized();
  assert(!normal.array().isNaN().any() && "normal is ill-defined");
 
  return normal;
}
 
inline Vec3s projectPointOnTriangle(const Vec3s& contact_point,
                                    const Triangle& triangle,
                                    const std::vector<Vec3s>& points) {
  const Vec3s pointA = points[triangle[0]];
  const Vec3s pointB = points[triangle[1]];
  const Vec3s pointC = points[triangle[2]];
 
  const Vec3s contact_point_projected =
      projectTriangle(pointA, pointB, pointC, contact_point);
 
  return contact_point_projected;
}
 
inline CoalScalar distanceContactPointToTriangle(
    const Vec3s& contact_point, const Triangle& triangle,
    const std::vector<Vec3s>& points) {
  const Vec3s contact_point_projected =
      projectPointOnTriangle(contact_point, triangle, points);
  return (contact_point_projected - contact_point).norm();
}
 
inline CoalScalar distanceContactPointToFace(const size_t face_id,
                                             const Vec3s& contact_point,
                                             const Convex<Triangle>& convex,
                                             size_t& closest_face_id) {
  assert((face_id >= 0 && face_id < 8) && "face_id should be in [0;7]");
 
  const std::vector<Vec3s>& points = *(convex.points);
  if (face_id <= 1) {
    const Triangle& triangle = (*(convex.polygons))[face_id];
    closest_face_id = face_id;
    return distanceContactPointToTriangle(contact_point, triangle, points);
  } else {
    const Triangle& triangle1 = (*(convex.polygons))[face_id];
    const CoalScalar distance_to_triangle1 =
        distanceContactPointToTriangle(contact_point, triangle1, points);
 
    const Triangle& triangle2 = (*(convex.polygons))[face_id + 1];
    const CoalScalar distance_to_triangle2 =
        distanceContactPointToTriangle(contact_point, triangle2, points);
 
    if (distance_to_triangle1 > distance_to_triangle2) {
      closest_face_id = face_id + 1;
      return distance_to_triangle2;
    } else {
      closest_face_id = face_id;
      return distance_to_triangle1;
    }
  }
}
 
template <typename Polygone, typename Shape>
bool binCorrection(const Convex<Polygone>& convex,
                   const int convex_active_faces, const Shape& shape,
                   const Transform3s& shape_pose, CoalScalar& distance,
                   Vec3s& contact_1, Vec3s& contact_2, Vec3s& normal,
                   Vec3s& face_normal, const bool is_collision) {
  const CoalScalar prec = 1e-12;
  const std::vector<Vec3s>& points = *(convex.points);
 
  bool hfield_witness_is_on_bin_side = true;
 
  //  int closest_face_id_bottom_face = -1;
  //  int closest_face_id_top_face = -1;
 
  std::vector<size_t> active_faces;
  active_faces.reserve(5);
  active_faces.push_back(0);
  active_faces.push_back(1);
 
  if (convex_active_faces & 2) active_faces.push_back(2);
  if (convex_active_faces & 4) active_faces.push_back(4);
  if (convex_active_faces & 8) active_faces.push_back(6);
 
  Triangle face_triangle;
  CoalScalar shortest_distance_to_face =
      (std::numeric_limits<CoalScalar>::max)();
  face_normal = normal;
  for (const size_t active_face : active_faces) {
    size_t closest_face_id;
    const CoalScalar distance_to_face = distanceContactPointToFace(
        active_face, contact_1, convex, closest_face_id);
 
    const bool contact_point_is_on_face = distance_to_face <= prec;
    if (contact_point_is_on_face) {
      hfield_witness_is_on_bin_side = false;
      face_triangle = (*(convex.polygons))[closest_face_id];
      shortest_distance_to_face = distance_to_face;
      break;
    } else if (distance_to_face < shortest_distance_to_face) {
      face_triangle = (*(convex.polygons))[closest_face_id];
      shortest_distance_to_face = distance_to_face;
    }
  }
 
  // We correct only if there is a collision with the bin
  if (is_collision) {
    if (!face_triangle.isValid())
      COAL_THROW_PRETTY("face_triangle is not initialized", std::logic_error);
 
    const Vec3s face_pointA = points[face_triangle[0]];
    face_normal = computeTriangleNormal(face_triangle, points);
 
    int hint = 0;
    // Since we compute the support manually, we need to take into account the
    // sphere swept radius of the shape.
    // TODO: take into account the swept-sphere radius of the bin.
    const Vec3s _support = getSupport<details::SupportOptions::WithSweptSphere>(
        &shape, -shape_pose.rotation().transpose() * face_normal, hint);
    const Vec3s support =
        shape_pose.rotation() * _support + shape_pose.translation();
 
    // Project support into the inclined bin having triangle
    const CoalScalar offset_plane = face_normal.dot(face_pointA);
    const Plane projection_plane(face_normal, offset_plane);
    const CoalScalar distance_support_projection_plane =
        projection_plane.signedDistance(support);
 
    const Vec3s projected_support =
        support - distance_support_projection_plane * face_normal;
 
    // We need now to project the projected in the triangle shape
    contact_1 =
        projectPointOnTriangle(projected_support, face_triangle, points);
    contact_2 = contact_1 + distance_support_projection_plane * face_normal;
    normal = face_normal;
    distance = -std::fabs(distance_support_projection_plane);
  }
 
  return hfield_witness_is_on_bin_side;
}
 
template <typename Polygone, typename Shape, int Options>
bool shapeDistance(const GJKSolver* nsolver, const CollisionRequest& request,
                   const Convex<Polygone>& convex1,
                   const int convex1_active_faces,
                   const Convex<Polygone>& convex2,
                   const int convex2_active_faces, const Transform3s& tf1,
                   const Shape& shape, const Transform3s& tf2,
                   CoalScalar& distance, Vec3s& c1, Vec3s& c2, Vec3s& normal,
                   Vec3s& normal_top, bool& hfield_witness_is_on_bin_side) {
  enum { RTIsIdentity = Options & RelativeTransformationIsIdentity };
 
  const Transform3s Id;
  // The solver `nsolver` has already been set up by the collision request
  // `request`. If GJK early stopping is enabled through `request`, it will be
  // used.
  // The only thing we need to make sure is that in case of collision, the
  // penetration information is computed (as we do bins comparison).
  const bool compute_penetration = true;
  Vec3s contact1_1, contact1_2, contact2_1, contact2_2;
  Vec3s normal1, normal1_top, normal2, normal2_top;
  CoalScalar distance1, distance2;
 
  if (RTIsIdentity) {
    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
        &convex1, Id, &shape, tf2, nsolver, compute_penetration, contact1_1,
        contact1_2, normal1);
  } else {
    distance1 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
        &convex1, tf1, &shape, tf2, nsolver, compute_penetration, contact1_1,
        contact1_2, normal1);
  }
  bool collision1 = (distance1 - request.security_margin <=
                     request.collision_distance_threshold);
 
  bool hfield_witness_is_on_bin_side1 =
      binCorrection(convex1, convex1_active_faces, shape, tf2, distance1,
                    contact1_1, contact1_2, normal1, normal1_top, collision1);
 
  if (RTIsIdentity) {
    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
        &convex2, Id, &shape, tf2, nsolver, compute_penetration, contact2_1,
        contact2_2, normal2);
  } else {
    distance2 = internal::ShapeShapeDistance<Convex<Polygone>, Shape>(
        &convex2, tf1, &shape, tf2, nsolver, compute_penetration, contact2_1,
        contact2_2, normal2);
  }
  bool collision2 = (distance2 - request.security_margin <=
                     request.collision_distance_threshold);
 
  bool hfield_witness_is_on_bin_side2 =
      binCorrection(convex2, convex2_active_faces, shape, tf2, distance2,
                    contact2_1, contact2_2, normal2, normal2_top, collision2);
 
  if (collision1 && collision2) {
    if (distance1 > distance2)  // switch values
    {
      distance = distance2;
      c1 = contact2_1;
      c2 = contact2_2;
      normal = normal2;
      normal_top = normal2_top;
      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
    } else {
      distance = distance1;
      c1 = contact1_1;
      c2 = contact1_2;
      normal = normal1;
      normal_top = normal1_top;
      hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
    }
    return true;
  } else if (collision1) {
    distance = distance1;
    c1 = contact1_1;
    c2 = contact1_2;
    normal = normal1;
    normal_top = normal1_top;
    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
    return true;
  } else if (collision2) {
    distance = distance2;
    c1 = contact2_1;
    c2 = contact2_2;
    normal = normal2;
    normal_top = normal2_top;
    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
    return true;
  }
 
  if (distance1 > distance2)  // switch values
  {
    distance = distance2;
    c1 = contact2_1;
    c2 = contact2_2;
    normal = normal2;
    normal_top = normal2_top;
    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side2;
  } else {
    distance = distance1;
    c1 = contact1_1;
    c2 = contact1_2;
    normal = normal1;
    normal_top = normal1_top;
    hfield_witness_is_on_bin_side = hfield_witness_is_on_bin_side1;
  }
  return false;
}
 
}  // namespace details
 
template <typename BV, typename S,
          int _Options = RelativeTransformationIsIdentity>
class HeightFieldShapeCollisionTraversalNode
    : public CollisionTraversalNodeBase {
 public:
  typedef CollisionTraversalNodeBase Base;
  typedef Eigen::Array<CoalScalar, 1, 2> Array2d;
 
  enum {
    Options = _Options,
    RTIsIdentity = _Options & RelativeTransformationIsIdentity
  };
 
  HeightFieldShapeCollisionTraversalNode(const CollisionRequest& request)
      : CollisionTraversalNodeBase(request) {
    model1 = NULL;
    model2 = NULL;
 
    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
 
    nsolver = NULL;
    count = 0;
  }
 
  bool isFirstNodeLeaf(unsigned int b) const {
    return model1->getBV(b).isLeaf();
  }
 
  int getFirstLeftChild(unsigned int b) const {
    return static_cast<int>(model1->getBV(b).leftChild());
  }
 
  int getFirstRightChild(unsigned int b) const {
    return static_cast<int>(model1->getBV(b).rightChild());
  }
 
  bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
                   CoalScalar& sqrDistLowerBound) const {
    if (this->enable_statistics) this->num_bv_tests++;
 
    bool disjoint;
    if (RTIsIdentity) {
      assert(false && "must never happened");
      disjoint = !this->model1->getBV(b1).bv.overlap(
          this->model2_bv, this->request, sqrDistLowerBound);
    } else {
      disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
                          this->model1->getBV(b1).bv, this->model2_bv,
                          this->request, sqrDistLowerBound);
    }
 
    if (disjoint)
      internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
                                               sqrDistLowerBound);
 
    assert(!disjoint || sqrDistLowerBound > 0);
    return disjoint;
  }
 
  void leafCollides(unsigned int b1, unsigned int /*b2*/,
                    CoalScalar& sqrDistLowerBound) const {
    count++;
    if (this->enable_statistics) this->num_leaf_tests++;
    const HFNode<BV>& node = this->model1->getBV(b1);
 
    // Split quadrilateral primitives into two convex shapes corresponding to
    // polyhedron with triangular bases. This is essential to keep the convexity
 
    //    typedef Convex<Quadrilateral> ConvexQuadrilateral;
    //    const ConvexQuadrilateral convex =
    //    details::buildConvexQuadrilateral(node,*this->model1);
 
    typedef Convex<Triangle> ConvexTriangle;
    ConvexTriangle convex1, convex2;
    int convex1_active_faces, convex2_active_faces;
    // TODO: inherit from hfield's inflation here
    details::buildConvexTriangles(node, *this->model1, convex1,
                                  convex1_active_faces, convex2,
                                  convex2_active_faces);
 
    // Compute aabb_local for BoundingVolumeGuess case in the GJK solver
    if (nsolver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
      convex1.computeLocalAABB();
      convex2.computeLocalAABB();
    }
 
    CoalScalar distance;
    //    Vec3s contact_point, normal;
    Vec3s c1, c2, normal, normal_face;
    bool hfield_witness_is_on_bin_side;
 
    bool collision = details::shapeDistance<Triangle, S, Options>(
        nsolver, this->request, convex1, convex1_active_faces, convex2,
        convex2_active_faces, this->tf1, *(this->model2), this->tf2, distance,
        c1, c2, normal, normal_face, hfield_witness_is_on_bin_side);
 
    CoalScalar distToCollision = distance - this->request.security_margin;
    if (distToCollision <= this->request.collision_distance_threshold) {
      sqrDistLowerBound = 0;
      if (this->result->numContacts() < this->request.num_max_contacts) {
        if (normal_face.isApprox(normal) &&
            (collision || !hfield_witness_is_on_bin_side)) {
          this->result->addContact(Contact(this->model1, this->model2, (int)b1,
                                           (int)Contact::NONE, c1, c2, normal,
                                           distance));
          assert(this->result->isCollision());
        }
      }
    } else
      sqrDistLowerBound = distToCollision * distToCollision;
 
    //    const Vec3s c1 = contact_point - distance * 0.5 * normal;
    //    const Vec3s c2 = contact_point + distance * 0.5 * normal;
    internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
                                               distToCollision, c1, c2, normal);
 
    assert(this->result->isCollision() || sqrDistLowerBound > 0);
  }
 
  const GJKSolver* nsolver;
 
  const HeightField<BV>* model1;
  const S* model2;
  BV model2_bv;
 
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable CoalScalar query_time_seconds;
  mutable int count;
};
 
 
 
template <typename BV, typename S,
          int _Options = RelativeTransformationIsIdentity>
class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
 public:
  typedef DistanceTraversalNodeBase Base;
 
  enum {
    Options = _Options,
    RTIsIdentity = _Options & RelativeTransformationIsIdentity
  };
 
  HeightFieldShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
    model1 = NULL;
    model2 = NULL;
 
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
 
    rel_err = 0;
    abs_err = 0;
    nsolver = NULL;
  }
 
  bool isFirstNodeLeaf(unsigned int b) const {
    return model1->getBV(b).isLeaf();
  }
 
  int getFirstLeftChild(unsigned int b) const {
    return model1->getBV(b).leftChild();
  }
 
  int getFirstRightChild(unsigned int b) const {
    return model1->getBV(b).rightChild();
  }
 
  CoalScalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
    return model1->getBV(b1).bv.distance(
        model2_bv);  // TODO(jcarpent): tf1 is not taken into account here.
  }
 
  void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
    if (this->enable_statistics) this->num_leaf_tests++;
 
    const BVNode<BV>& node = this->model1->getBV(b1);
 
    typedef Convex<Quadrilateral> ConvexQuadrilateral;
    const ConvexQuadrilateral convex =
        details::buildConvexQuadrilateral(node, *this->model1);
 
    Vec3s p1, p2, normal;
    const CoalScalar distance =
        internal::ShapeShapeDistance<ConvexQuadrilateral, S>(
            &convex, this->tf1, this->model2, this->tf2, this->nsolver,
            this->request.enable_signed_distance, p1, p2, normal);
 
    this->result->update(distance, this->model1, this->model2, b1,
                         DistanceResult::NONE, p1, p2, normal);
  }
 
  bool canStop(CoalScalar c) const {
    if ((c >= this->result->min_distance - abs_err) &&
        (c * (1 + rel_err) >= this->result->min_distance))
      return true;
    return false;
  }
 
  CoalScalar rel_err;
  CoalScalar abs_err;
 
  const GJKSolver* nsolver;
 
  const HeightField<BV>* model1;
  const S* model2;
  BV model2_bv;
 
  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable CoalScalar query_time_seconds;
};
 
 
}  // namespace coal
 
 
#endif