Program Listing for File traversal_node_bvhs.h
↰ Return to documentation for file (/tmp/ws/src/hpp-fcl/include/hpp/fcl/internal/traversal_node_bvhs.h
)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, 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.
*/
#ifndef HPP_FCL_TRAVERSAL_NODE_MESHES_H
#define HPP_FCL_TRAVERSAL_NODE_MESHES_H
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/internal/traversal_node_base.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/internal/intersect.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/internal/traversal.h>
#include <limits>
#include <vector>
#include <cassert>
namespace hpp {
namespace fcl {
template <typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
public:
BVHCollisionTraversalNode(const CollisionRequest& request)
: CollisionTraversalNodeBase(request) {
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
bool isFirstNodeLeaf(unsigned int b) const {
assert(model1 != NULL && "model1 is NULL");
return model1->getBV(b).isLeaf();
}
bool isSecondNodeLeaf(unsigned int b) const {
assert(model2 != NULL && "model2 is NULL");
return model2->getBV(b).isLeaf();
}
bool firstOverSecond(unsigned int b1, unsigned int b2) const {
FCL_REAL sz1 = model1->getBV(b1).bv.size();
FCL_REAL sz2 = model2->getBV(b2).bv.size();
bool l1 = model1->getBV(b1).isLeaf();
bool l2 = model2->getBV(b2).isLeaf();
if (l2 || (!l1 && (sz1 > sz2))) return true;
return false;
}
int getFirstLeftChild(unsigned int b) const {
return model1->getBV(b).leftChild();
}
int getFirstRightChild(unsigned int b) const {
return model1->getBV(b).rightChild();
}
int getSecondLeftChild(unsigned int b) const {
return model2->getBV(b).leftChild();
}
int getSecondRightChild(unsigned int b) const {
return model2->getBV(b).rightChild();
}
const BVHModel<BV>* model1;
const BVHModel<BV>* model2;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
template <typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
MeshCollisionTraversalNode(const CollisionRequest& request)
: BVHCollisionTraversalNode<BV>(request) {
vertices1 = NULL;
vertices2 = NULL;
tri_indices1 = NULL;
tri_indices2 = NULL;
}
bool BVDisjoints(unsigned int b1, unsigned int b2,
FCL_REAL& sqrDistLowerBound) const {
if (this->enable_statistics) this->num_bv_tests++;
bool disjoint;
if (RTIsIdentity)
disjoint = !this->model1->getBV(b1).overlap(
this->model2->getBV(b2), this->request, sqrDistLowerBound);
else {
disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
this->model1->getBV(b1).bv, this->request,
sqrDistLowerBound);
}
if (disjoint)
internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
sqrDistLowerBound);
return disjoint;
}
void leafCollides(unsigned int b1, unsigned int b2,
FCL_REAL& sqrDistLowerBound) const {
if (this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
int primitive_id1 = node1.primitiveId();
int primitive_id2 = node2.primitiveId();
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
const Vec3f& P1 = vertices1[tri_id1[0]];
const Vec3f& P2 = vertices1[tri_id1[1]];
const Vec3f& P3 = vertices1[tri_id1[2]];
const Vec3f& Q1 = vertices2[tri_id2[0]];
const Vec3f& Q2 = vertices2[tri_id2[1]];
const Vec3f& Q3 = vertices2[tri_id2[2]];
TriangleP tri1(P1, P2, P3);
TriangleP tri2(Q1, Q2, Q3);
GJKSolver solver;
Vec3f p1,
p2; // closest points if no collision contact points if collision.
Vec3f normal;
FCL_REAL distance;
solver.shapeDistance(tri1, this->tf1, tri2, this->tf2, distance, p1, p2,
normal);
const FCL_REAL distToCollision = distance - this->request.security_margin;
if (distToCollision <=
this->request.collision_distance_threshold) { // collision
sqrDistLowerBound = 0;
Vec3f p(p1); // contact point
if (this->result->numContacts() < this->request.num_max_contacts) {
// How much (Q1, Q2, Q3) should be moved so that all vertices are
// above (P1, P2, P3).
if (distance > 0) {
normal = (p2 - p1).normalized();
p = .5 * (p1 + p2);
}
this->result->addContact(Contact(this->model1, this->model2,
primitive_id1, primitive_id2, p,
normal, -distance));
}
} else
sqrDistLowerBound = distToCollision * distToCollision;
internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result,
distToCollision, p1, p2);
}
Vec3f* vertices1;
Vec3f* vertices2;
Triangle* tri_indices1;
Triangle* tri_indices2;
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
};
typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
namespace details {
template <typename BV>
struct DistanceTraversalBVDistanceLowerBound_impl {
static FCL_REAL run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
return b1.distance(b2);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<BV>& b1,
const BVNode<BV>& b2) {
return distance(R, T, b1.bv, b2.bv);
}
};
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
static FCL_REAL run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
FCL_REAL sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (b1.overlap(b2, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt(sqrDistLowerBound);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<OBB>& b1,
const BVNode<OBB>& b2) {
FCL_REAL sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt(sqrDistLowerBound);
}
};
template <>
struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
static FCL_REAL run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
FCL_REAL sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (b1.overlap(b2, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt(sqrDistLowerBound);
}
static FCL_REAL run(const Matrix3f& R, const Vec3f& T, const BVNode<AABB>& b1,
const BVNode<AABB>& b2) {
FCL_REAL sqrDistLowerBound;
CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
// request.break_distance = ?
if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
// TODO A penetration upper bound should be computed.
return -1;
}
return sqrt(sqrDistLowerBound);
}
};
} // namespace details
template <typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
public:
BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
model1 = NULL;
model2 = NULL;
num_bv_tests = 0;
num_leaf_tests = 0;
query_time_seconds = 0.0;
}
bool isFirstNodeLeaf(unsigned int b) const {
return model1->getBV(b).isLeaf();
}
bool isSecondNodeLeaf(unsigned int b) const {
return model2->getBV(b).isLeaf();
}
bool firstOverSecond(unsigned int b1, unsigned int b2) const {
FCL_REAL sz1 = model1->getBV(b1).bv.size();
FCL_REAL sz2 = model2->getBV(b2).bv.size();
bool l1 = model1->getBV(b1).isLeaf();
bool l2 = model2->getBV(b2).isLeaf();
if (l2 || (!l1 && (sz1 > sz2))) return true;
return false;
}
int getFirstLeftChild(unsigned int b) const {
return model1->getBV(b).leftChild();
}
int getFirstRightChild(unsigned int b) const {
return model1->getBV(b).rightChild();
}
int getSecondLeftChild(unsigned int b) const {
return model2->getBV(b).leftChild();
}
int getSecondRightChild(unsigned int b) const {
return model2->getBV(b).rightChild();
}
const BVHModel<BV>* model1;
const BVHModel<BV>* model2;
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
};
template <typename BV, int _Options = RelativeTransformationIsIdentity>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
public:
enum {
Options = _Options,
RTIsIdentity = _Options & RelativeTransformationIsIdentity
};
using BVHDistanceTraversalNode<BV>::enable_statistics;
using BVHDistanceTraversalNode<BV>::request;
using BVHDistanceTraversalNode<BV>::result;
using BVHDistanceTraversalNode<BV>::tf1;
using BVHDistanceTraversalNode<BV>::model1;
using BVHDistanceTraversalNode<BV>::model2;
using BVHDistanceTraversalNode<BV>::num_bv_tests;
using BVHDistanceTraversalNode<BV>::num_leaf_tests;
MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
vertices1 = NULL;
vertices2 = NULL;
tri_indices1 = NULL;
tri_indices2 = NULL;
rel_err = this->request.rel_err;
abs_err = this->request.abs_err;
}
void preprocess() {
if (!RTIsIdentity) preprocessOrientedNode();
}
void postprocess() {
if (!RTIsIdentity) postprocessOrientedNode();
}
FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
if (enable_statistics) num_bv_tests++;
if (RTIsIdentity)
return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
model1->getBV(b1), model2->getBV(b2));
else
return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
}
void leafComputeDistance(unsigned int b1, unsigned int b2) const {
if (this->enable_statistics) this->num_leaf_tests++;
const BVNode<BV>& node1 = this->model1->getBV(b1);
const BVNode<BV>& node2 = this->model2->getBV(b2);
int primitive_id1 = node1.primitiveId();
int primitive_id2 = node2.primitiveId();
const Triangle& tri_id1 = tri_indices1[primitive_id1];
const Triangle& tri_id2 = tri_indices2[primitive_id2];
const Vec3f& t11 = vertices1[tri_id1[0]];
const Vec3f& t12 = vertices1[tri_id1[1]];
const Vec3f& t13 = vertices1[tri_id1[2]];
const Vec3f& t21 = vertices2[tri_id2[0]];
const Vec3f& t22 = vertices2[tri_id2[1]];
const Vec3f& t23 = vertices2[tri_id2[2]];
// nearest point pair
Vec3f P1, P2, normal;
FCL_REAL d2;
if (RTIsIdentity)
d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
P2);
else
d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
RT._R(), RT._T(), P1, P2);
FCL_REAL d = sqrt(d2);
this->result->update(d, this->model1, this->model2, primitive_id1,
primitive_id2, P1, P2, normal);
}
bool canStop(FCL_REAL c) const {
if ((c >= this->result->min_distance - abs_err) &&
(c * (1 + rel_err) >= this->result->min_distance))
return true;
return false;
}
Vec3f* vertices1;
Vec3f* vertices2;
Triangle* tri_indices1;
Triangle* tri_indices2;
FCL_REAL rel_err;
FCL_REAL abs_err;
details::RelativeTransformation<!bool(RTIsIdentity)> RT;
private:
void preprocessOrientedNode() {
const int init_tri_id1 = 0, init_tri_id2 = 0;
const Triangle& init_tri1 = tri_indices1[init_tri_id1];
const Triangle& init_tri2 = tri_indices2[init_tri_id2];
Vec3f init_tri1_points[3];
Vec3f init_tri2_points[3];
init_tri1_points[0] = vertices1[init_tri1[0]];
init_tri1_points[1] = vertices1[init_tri1[1]];
init_tri1_points[2] = vertices1[init_tri1[2]];
init_tri2_points[0] = vertices2[init_tri2[0]];
init_tri2_points[1] = vertices2[init_tri2[1]];
init_tri2_points[2] = vertices2[init_tri2[2]];
Vec3f p1, p2, normal;
FCL_REAL distance = sqrt(TriangleDistance::sqrTriDistance(
init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
RT._T(), p1, p2));
result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
normal);
}
void postprocessOrientedNode() {
if (request.enable_nearest_points && (result->o1 == model1) &&
(result->o2 == model2)) {
result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
}
}
};
typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
namespace details {
template <typename BV>
inline const Matrix3f& getBVAxes(const BV& bv) {
return bv.axes;
}
template <>
inline const Matrix3f& getBVAxes<OBBRSS>(const OBBRSS& bv) {
return bv.obb.axes;
}
} // namespace details
} // namespace fcl
} // namespace hpp
#endif