box_plane.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * Copyright (c) 2018-2019, Center National de la Recherche Scientifique
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #include <cmath>
40 #include <limits>
41 #include <hpp/fcl/math/transform.h>
43 
45 #include "../narrowphase/details.h"
46 
47 namespace hpp {
48 namespace fcl {
49 struct GJKSolver;
50 
51 template <>
53  const CollisionGeometry* o1, const Transform3f& tf1,
54  const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
55  const DistanceRequest&, DistanceResult& result) {
56  const Box& s1 = static_cast<const Box&>(*o1);
57  const Plane& s2 = static_cast<const Plane&>(*o2);
58  details::boxPlaneIntersect(s1, tf1, s2, tf2, result.min_distance,
59  result.nearest_points[0], result.nearest_points[1],
60  result.normal);
61  result.o1 = o1;
62  result.o2 = o2;
63  result.b1 = -1;
64  result.b2 = -1;
65  return result.min_distance;
66 }
67 
68 template <>
70  const CollisionGeometry* o1, const Transform3f& tf1,
71  const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*,
72  const DistanceRequest&, DistanceResult& result) {
73  const Plane& s1 = static_cast<const Plane&>(*o1);
74  const Box& s2 = static_cast<const Box&>(*o2);
75  details::boxPlaneIntersect(s2, tf2, s1, tf1, result.min_distance,
76  result.nearest_points[1], result.nearest_points[0],
77  result.normal);
78  result.o1 = o1;
79  result.o2 = o2;
80  result.b1 = -1;
81  result.b2 = -1;
82  result.normal = -result.normal;
83  return result.min_distance;
84 }
85 } // namespace fcl
86 
87 } // namespace hpp
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
request to the distance computation
Main namespace.
tuple tf2
bool boxPlaneIntersect(const Box &s1, const Transform3f &tf1, const Plane &s2, const Transform3f &tf2, FCL_REAL &distance, Vec3f &p1, Vec3f &p2, Vec3f &normal)
box half space, a, b, c = +/- edge size n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d so (R^T n) (a v1 + ...
Definition: details.h:1879
double FCL_REAL
Definition: data_types.h:65
Center at zero point, axis aligned box.
collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the ...
Definition: narrowphase.h:54
FCL_REAL ShapeShapeDistance< Plane, Box >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_plane.cpp:69
The geometry for the object for collision or distance computation.
FCL_REAL ShapeShapeDistance< Box, Plane >(const CollisionGeometry *o1, const Transform3f &tf1, const CollisionGeometry *o2, const Transform3f &tf2, const GJKSolver *, const DistanceRequest &, DistanceResult &result)
Definition: box_plane.cpp:52
tuple tf1


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00