GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/hpp/fcl/internal/intersect.h Lines: 1 1 100.0 %
Date: 2024-02-09 12:57:42 Branches: 0 0 - %

Line Branch Exec Source
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
 *  All rights reserved.
7
 *
8
 *  Redistribution and use in source and binary forms, with or without
9
 *  modification, are permitted provided that the following conditions
10
 *  are met:
11
 *
12
 *   * Redistributions of source code must retain the above copyright
13
 *     notice, this list of conditions and the following disclaimer.
14
 *   * Redistributions in binary form must reproduce the above
15
 *     copyright notice, this list of conditions and the following
16
 *     disclaimer in the documentation and/or other materials provided
17
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
 *     contributors may be used to endorse or promote products derived
20
 *     from this software without specific prior written permission.
21
 *
22
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
 *  POSSIBILITY OF SUCH DAMAGE.
34
 */
35
36
/** \author Jia Pan */
37
38
#ifndef HPP_FCL_INTERSECT_H
39
#define HPP_FCL_INTERSECT_H
40
41
/// @cond INTERNAL
42
43
#include <hpp/fcl/math/transform.h>
44
45
namespace hpp {
46
namespace fcl {
47
48
/// @brief CCD intersect kernel among primitives
49
class HPP_FCL_DLLAPI Intersect {
50
 public:
51
  static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2,
52
                                 const Vec3f& v3, Vec3f* n, FCL_REAL* t);
53
};  // class Intersect
54
55
/// @brief Project functions
56
class HPP_FCL_DLLAPI Project {
57
 public:
58
  struct HPP_FCL_DLLAPI ProjectResult {
59
    /// @brief Parameterization of the projected point (based on the simplex to
60
    /// be projected, use 2 or 3 or 4 of the array)
61
    FCL_REAL parameterization[4];
62
63
    /// @brief square distance from the query point to the projected simplex
64
    FCL_REAL sqr_distance;
65
66
    /// @brief the code of the projection type
67
    unsigned int encode;
68
69
30368680
    ProjectResult() : sqr_distance(-1), encode(0) {}
70
  };
71
72
  /// @brief Project point p onto line a-b
73
  static ProjectResult projectLine(const Vec3f& a, const Vec3f& b,
74
                                   const Vec3f& p);
75
76
  /// @brief Project point p onto triangle a-b-c
77
  static ProjectResult projectTriangle(const Vec3f& a, const Vec3f& b,
78
                                       const Vec3f& c, const Vec3f& p);
79
80
  /// @brief Project point p onto tetrahedra a-b-c-d
81
  static ProjectResult projectTetrahedra(const Vec3f& a, const Vec3f& b,
82
                                         const Vec3f& c, const Vec3f& d,
83
                                         const Vec3f& p);
84
85
  /// @brief Project origin (0) onto line a-b
86
  static ProjectResult projectLineOrigin(const Vec3f& a, const Vec3f& b);
87
88
  /// @brief Project origin (0) onto triangle a-b-c
89
  static ProjectResult projectTriangleOrigin(const Vec3f& a, const Vec3f& b,
90
                                             const Vec3f& c);
91
92
  /// @brief Project origin (0) onto tetrahedran a-b-c-d
93
  static ProjectResult projectTetrahedraOrigin(const Vec3f& a, const Vec3f& b,
94
                                               const Vec3f& c, const Vec3f& d);
95
};
96
97
/// @brief Triangle distance functions
98
class HPP_FCL_DLLAPI TriangleDistance {
99
 public:
100
  /// @brief Returns closest points between an segment pair.
101
  /// The first segment is P + t * A
102
  /// The second segment is Q + t * B
103
  /// X, Y are the closest points on the two segments
104
  /// VEC is the vector between X and Y
105
  static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q,
106
                        const Vec3f& B, Vec3f& VEC, Vec3f& X, Vec3f& Y);
107
108
  /// Compute squared distance between triangles
109
  /// @param S and T are two triangles
110
  /// @retval P, Q closest points if triangles do not intersect.
111
  /// @return squared distance if triangles do not intersect, 0 otherwise.
112
  /// If the triangles are disjoint, P and Q give the closet points of
113
  /// S and T respectively. However,
114
  /// if the triangles overlap, P and Q are basically a random pair of points
115
  /// from the triangles, not coincident points on the intersection of the
116
  /// triangles, as might be expected.
117
  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P,
118
                                 Vec3f& Q);
119
120
  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
121
                                 const Vec3f& S3, const Vec3f& T1,
122
                                 const Vec3f& T2, const Vec3f& T3, Vec3f& P,
123
                                 Vec3f& Q);
124
125
  /// Compute squared distance between triangles
126
  /// @param S and T are two triangles
127
  /// @param R, Tl, rotation and translation applied to T,
128
  /// @retval P, Q closest points if triangles do not intersect.
129
  /// @return squared distance if triangles do not intersect, 0 otherwise.
130
  /// If the triangles are disjoint, P and Q give the closet points of
131
  /// S and T respectively. However,
132
  /// if the triangles overlap, P and Q are basically a random pair of points
133
  /// from the triangles, not coincident points on the intersection of the
134
  /// triangles, as might be expected.
135
  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
136
                                 const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
137
                                 Vec3f& Q);
138
139
  /// Compute squared distance between triangles
140
  /// @param S and T are two triangles
141
  /// @param tf, rotation and translation applied to T,
142
  /// @retval P, Q closest points if triangles do not intersect.
143
  /// @return squared distance if triangles do not intersect, 0 otherwise.
144
  /// If the triangles are disjoint, P and Q give the closet points of
145
  /// S and T respectively. However,
146
  /// if the triangles overlap, P and Q are basically a random pair of points
147
  /// from the triangles, not coincident points on the intersection of the
148
  /// triangles, as might be expected.
149
  static FCL_REAL sqrTriDistance(const Vec3f S[3], const Vec3f T[3],
150
                                 const Transform3f& tf, Vec3f& P, Vec3f& Q);
151
152
  /// Compute squared distance between triangles
153
  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
154
  /// @param R, Tl, rotation and translation applied to T1, T2, T3,
155
  /// @retval P, Q closest points if triangles do not intersect.
156
  /// @return squared distance if triangles do not intersect, 0 otherwise.
157
  /// If the triangles are disjoint, P and Q give the closet points of
158
  /// S and T respectively. However,
159
  /// if the triangles overlap, P and Q are basically a random pair of points
160
  /// from the triangles, not coincident points on the intersection of the
161
  /// triangles, as might be expected.
162
  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
163
                                 const Vec3f& S3, const Vec3f& T1,
164
                                 const Vec3f& T2, const Vec3f& T3,
165
                                 const Matrix3f& R, const Vec3f& Tl, Vec3f& P,
166
                                 Vec3f& Q);
167
168
  /// Compute squared distance between triangles
169
  /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
170
  /// @param tf, rotation and translation applied to T1, T2, T3,
171
  /// @retval P, Q closest points if triangles do not intersect.
172
  /// @return squared distance if triangles do not intersect, 0 otherwise.
173
  /// If the triangles are disjoint, P and Q give the closet points of
174
  /// S and T respectively. However,
175
  /// if the triangles overlap, P and Q are basically a random pair of points
176
  /// from the triangles, not coincident points on the intersection of the
177
  /// triangles, as might be expected.
178
  static FCL_REAL sqrTriDistance(const Vec3f& S1, const Vec3f& S2,
179
                                 const Vec3f& S3, const Vec3f& T1,
180
                                 const Vec3f& T2, const Vec3f& T3,
181
                                 const Transform3f& tf, Vec3f& P, Vec3f& Q);
182
};
183
184
}  // namespace fcl
185
186
}  // namespace hpp
187
188
/// @endcond
189
190
#endif