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