38 #ifndef FCL_TRAVERSAL_SHAPEMESHCOLLISIONTRAVERSALNODE_INL_H
39 #define FCL_TRAVERSAL_SHAPEMESHCOLLISIONTRAVERSALNODE_INL_H
52 template <
typename Shape,
typename BV,
typename NarrowPhaseSolver>
58 tri_indices =
nullptr;
64 template <
typename Shape,
typename BV,
typename NarrowPhaseSolver>
70 using S =
typename BV::S;
72 if(this->enable_statistics) this->num_leaf_tests++;
73 const BVNode<BV>& node = this->model2->getBV(b2);
77 const Triangle& tri_id = tri_indices[primitive_id];
83 if(this->model1->isOccupied() && this->model2->isOccupied())
85 bool is_intersect =
false;
87 if(!this->request.enable_contact)
89 if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3,
nullptr,
nullptr,
nullptr))
92 if(this->request.num_max_contacts > this->result->numContacts())
102 if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
105 if(this->request.num_max_contacts > this->result->numContacts())
106 this->result->addContact(
Contact<S>(this->model1, this->model2,
Contact<S>::NONE, primitive_id, contactp, normal, penetration));
110 if(is_intersect && this->request.enable_cost)
114 computeBV(*(this->model1), this->tf1, shape_aabb);
116 this->result->addCostSource(
CostSource<S>(overlap_part, cost_density), this->request.num_max_cost_sources);
119 else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
121 if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3,
nullptr,
nullptr,
nullptr))
125 computeBV(*(this->model1), this->tf1, shape_aabb);
127 this->result->addCostSource(
CostSource<S>(overlap_part, cost_density), this->request.num_max_cost_sources);
133 template <
typename Shape,
typename BV,
typename NarrowPhaseSolver>
137 return this->request.isSatisfied(*(this->result));
141 template <
typename Shape,
typename BV,
typename NarrowPhaseSolver>
149 const NarrowPhaseSolver* nsolver,
155 using S =
typename BV::S;
160 if(!tf2.matrix().isIdentity())
162 std::vector<Vector3<S>> vertices_transformed(model2.
num_vertices);
167 vertices_transformed[i] = new_v;
197 template <
typename Shape,
typename NarrowPhaseSolver>
204 template <
typename Shape,
typename NarrowPhaseSolver>
210 if(this->enable_statistics) this->num_bv_tests++;
211 return !
overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv);
215 template <
typename Shape,
typename NarrowPhaseSolver>
220 this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
226 template <
typename Shape,
typename NarrowPhaseSolver>
233 template <
typename Shape,
typename NarrowPhaseSolver>
239 if(this->enable_statistics) this->num_bv_tests++;
240 return !
overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv);
244 template <
typename Shape,
typename NarrowPhaseSolver>
249 this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
255 template <
typename Shape,
typename NarrowPhaseSolver>
262 template <
typename Shape,
typename NarrowPhaseSolver>
268 if(this->enable_statistics) this->num_bv_tests++;
269 return !
overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv);
273 template <
typename Shape,
typename NarrowPhaseSolver>
278 this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
284 template <
typename Shape,
typename NarrowPhaseSolver>
291 template <
typename Shape,
typename NarrowPhaseSolver>
297 if(this->enable_statistics) this->num_bv_tests++;
298 return !
overlap(this->tf2.linear(), this->tf2.translation(), this->model1_bv, this->model2->getBV(b2).bv);
302 template <
typename Shape,
typename NarrowPhaseSolver>
307 this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
312 template <
typename Shape,
typename BV,
typename NarrowPhaseSolver,
template <
typename,
typename>
class OrientedNode>
316 const NarrowPhaseSolver* nsolver,
323 node.model1 = &model1;
325 node.model2 = &model2;
327 node.nsolver = nsolver;
334 node.request = request;
335 node.result = &result;
337 node.cost_density = model1.cost_density * model2.
cost_density;
343 template <
typename Shape,
typename NarrowPhaseSolver>
351 const NarrowPhaseSolver* nsolver,
356 node, model1, tf1, model2, tf2, nsolver, request, result);
360 template <
typename Shape,
typename NarrowPhaseSolver>
368 const NarrowPhaseSolver* nsolver,
373 node, model1, tf1, model2, tf2, nsolver, request, result);
377 template <
typename Shape,
typename NarrowPhaseSolver>
385 const NarrowPhaseSolver* nsolver,
390 node, model1, tf1, model2, tf2, nsolver, request, result);
394 template <
typename Shape,
typename NarrowPhaseSolver>
402 const NarrowPhaseSolver* nsolver,
407 node, model1, tf1, model2, tf2, nsolver, request, result);