GCC Code Coverage Report

Directory: ./
File: include/coal/internal/intersect.h
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 1 1 100.0%
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 COAL_INTERSECT_H
39 #define COAL_INTERSECT_H
40
41 /// @cond INTERNAL
42
43 #include "coal/math/transform.h"
44
45 namespace coal {
46
47 /// @brief CCD intersect kernel among primitives
48 class COAL_DLLAPI Intersect {
49 public:
50 static bool buildTrianglePlane(const Vec3s& v1, const Vec3s& v2,
51 const Vec3s& v3, Vec3s* n, Scalar* t);
52 }; // class Intersect
53
54 /// @brief Project functions
55 template <typename _Scalar>
56 class Project {
57 public:
58 typedef _Scalar Scalar;
59 typedef Eigen::Matrix<Scalar, 3, 1> Vec3;
60
61 struct ProjectResult {
62 /// @brief Parameterization of the projected point (based on the simplex to
63 /// be projected, use 2 or 3 or 4 of the array)
64 Scalar parameterization[4];
65
66 /// @brief square distance from the query point to the projected simplex
67 Scalar sqr_distance;
68
69 /// @brief the code of the projection type
70 unsigned int encode;
71
72 30562207 ProjectResult() : sqr_distance(-1), encode(0) {}
73 };
74
75 /// @brief Project point p onto line a-b
76 static ProjectResult projectLine(const Vec3& a, const Vec3& b, const Vec3& p);
77
78 /// @brief Project point p onto triangle a-b-c
79 static ProjectResult projectTriangle(const Vec3& a, const Vec3& b,
80 const Vec3& c, const Vec3& p);
81
82 /// @brief Project point p onto tetrahedra a-b-c-d
83 static ProjectResult projectTetrahedra(const Vec3& a, const Vec3& b,
84 const Vec3& c, const Vec3& d,
85 const Vec3& p);
86
87 /// @brief Project origin (0) onto line a-b
88 static ProjectResult projectLineOrigin(const Vec3& a, const Vec3& b);
89
90 /// @brief Project origin (0) onto triangle a-b-c
91 static ProjectResult projectTriangleOrigin(const Vec3& a, const Vec3& b,
92 const Vec3& c);
93
94 /// @brief Project origin (0) onto tetrahedran a-b-c-d
95 static ProjectResult projectTetrahedraOrigin(const Vec3& a, const Vec3& b,
96 const Vec3& c, const Vec3& d);
97 };
98
99 /// @brief Triangle distance functions
100 class COAL_DLLAPI TriangleDistance {
101 public:
102 /// @brief Returns closest points between an segment pair.
103 /// The first segment is P + t * A
104 /// The second segment is Q + t * B
105 /// X, Y are the closest points on the two segments
106 /// VEC is the vector between X and Y
107 static void segPoints(const Vec3s& P, const Vec3s& A, const Vec3s& Q,
108 const Vec3s& B, Vec3s& VEC, Vec3s& X, Vec3s& Y);
109
110 /// Compute squared distance between triangles
111 /// @param S and T are two triangles
112 /// @retval P, Q closest points if triangles do not intersect.
113 /// @return squared distance if triangles do not intersect, 0 otherwise.
114 /// If the triangles are disjoint, P and Q give the closet points of
115 /// S and T respectively. However,
116 /// if the triangles overlap, P and Q are basically a random pair of points
117 /// from the triangles, not coincident points on the intersection of the
118 /// triangles, as might be expected.
119 static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3], Vec3s& P,
120 Vec3s& Q);
121
122 static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
123 const Vec3s& S3, const Vec3s& T1,
124 const Vec3s& T2, const Vec3s& T3, Vec3s& P,
125 Vec3s& Q);
126
127 /// Compute squared distance between triangles
128 /// @param S and T are two triangles
129 /// @param R, Tl, rotation and translation applied to T,
130 /// @retval P, Q closest points if triangles do not intersect.
131 /// @return squared distance if triangles do not intersect, 0 otherwise.
132 /// If the triangles are disjoint, P and Q give the closet points of
133 /// S and T respectively. However,
134 /// if the triangles overlap, P and Q are basically a random pair of points
135 /// from the triangles, not coincident points on the intersection of the
136 /// triangles, as might be expected.
137 static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
138 const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
139 Vec3s& Q);
140
141 /// Compute squared distance between triangles
142 /// @param S and T are two triangles
143 /// @param tf, rotation and translation applied to T,
144 /// @retval P, Q closest points if triangles do not intersect.
145 /// @return squared distance if triangles do not intersect, 0 otherwise.
146 /// If the triangles are disjoint, P and Q give the closet points of
147 /// S and T respectively. However,
148 /// if the triangles overlap, P and Q are basically a random pair of points
149 /// from the triangles, not coincident points on the intersection of the
150 /// triangles, as might be expected.
151 static Scalar sqrTriDistance(const Vec3s S[3], const Vec3s T[3],
152 const Transform3s& tf, Vec3s& P, Vec3s& Q);
153
154 /// Compute squared distance between triangles
155 /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
156 /// @param R, Tl, rotation and translation applied to T1, T2, T3,
157 /// @retval P, Q closest points if triangles do not intersect.
158 /// @return squared distance if triangles do not intersect, 0 otherwise.
159 /// If the triangles are disjoint, P and Q give the closet points of
160 /// S and T respectively. However,
161 /// if the triangles overlap, P and Q are basically a random pair of points
162 /// from the triangles, not coincident points on the intersection of the
163 /// triangles, as might be expected.
164 static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
165 const Vec3s& S3, const Vec3s& T1,
166 const Vec3s& T2, const Vec3s& T3,
167 const Matrix3s& R, const Vec3s& Tl, Vec3s& P,
168 Vec3s& Q);
169
170 /// Compute squared distance between triangles
171 /// @param S1, S2, S3 and T1, T2, T3 are triangle vertices
172 /// @param tf, rotation and translation applied to T1, T2, T3,
173 /// @retval P, Q closest points if triangles do not intersect.
174 /// @return squared distance if triangles do not intersect, 0 otherwise.
175 /// If the triangles are disjoint, P and Q give the closet points of
176 /// S and T respectively. However,
177 /// if the triangles overlap, P and Q are basically a random pair of points
178 /// from the triangles, not coincident points on the intersection of the
179 /// triangles, as might be expected.
180 static Scalar sqrTriDistance(const Vec3s& S1, const Vec3s& S2,
181 const Vec3s& S3, const Vec3s& T1,
182 const Vec3s& T2, const Vec3s& T3,
183 const Transform3s& tf, Vec3s& P, Vec3s& Q);
184 };
185
186 } // namespace coal
187
188 /// @endcond
189
190 #include "coal/internal/intersect.hxx"
191
192 #endif
193