Skip to content

Commit 83ff99b

Browse files
Q-MinhQ-Minh
committed
Document DistanceQueries
1 parent 9f6e0ee commit 83ff99b

File tree

2 files changed

+127
-66
lines changed

2 files changed

+127
-66
lines changed

‎source/pbat/geometry/ClosestPointQueries.h‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,9 @@ namespace mini = math::linalg::mini;
2828

2929
/**
3030
* @brief Obtain the point on the plane (P,n) closest to the point X.
31+
* @tparam TMatrixX Query point matrix type
32+
* @tparam TMatrixP Point on the plane matrix type
33+
* @tparam TMatrixN Normal of the plane matrix type
3134
* @param X Query point
3235
* @param P Point on the plane
3336
* @param n Normal of the plane
@@ -39,6 +42,9 @@ PBAT_HOST_DEVICE auto PointOnPlane(TMatrixX const& X, TMatrixP const& P, TMatrix
3942

4043
/**
4144
* @brief Obtain the point on the line segment PQ closest to the point X.
45+
* @tparam TMatrixX Query point matrix type
46+
* @tparam TMatrixP Start point of the line segment matrix type
47+
* @tparam TMatrixQ End point of the line segment matrix type
4248
* @param X Query point
4349
* @param P Start point of the line segment
4450
* @param Q End point of the line segment
@@ -51,6 +57,9 @@ PBAT_HOST_DEVICE auto PointOnLineSegment(TMatrixX const& X, TMatrixP const& P, T
5157
/**
5258
* @brief Obtain the point on the axis-aligned bounding box (AABB) defined by the lower
5359
* and upper corners closest to the point X.
60+
* @tparam TMatrixX Query point matrix type
61+
* @tparam TMatrixL Lower corner of the AABB matrix type
62+
* @tparam TMatrixU Upper corner of the AABB matrix type
5463
* @param X Query point
5564
* @param L Lower corner of the AABB
5665
* @param U Upper corner of the AABB
@@ -63,6 +72,10 @@ PointOnAxisAlignedBoundingBox(TMatrixX const& X, TMatrixL const& L, TMatrixU con
6372

6473
/**
6574
* @brief Obtain the point on the triangle ABC closest to the point P in barycentric coordinates.
75+
* @tparam TMatrixP Query point matrix type
76+
* @tparam TMatrixA Vertex A of the triangle matrix type
77+
* @tparam TMatrixB Vertex B of the triangle matrix type
78+
* @tparam TMatrixC Vertex C of the triangle matrix type
6679
* @param P Query point
6780
* @param A Vertex A of the triangle
6881
* @param B Vertex B of the triangle
@@ -80,6 +93,10 @@ UvwPointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMat
8093

8194
/**
8295
* @brief Obtain the point on the triangle ABC closest to the point P.
96+
* @tparam TMatrixP Query point matrix type
97+
* @tparam TMatrixA Vertex A of the triangle matrix type
98+
* @tparam TMatrixB Vertex B of the triangle matrix type
99+
* @tparam TMatrixC Vertex C of the triangle matrix type
83100
* @param P Query point
84101
* @param A Vertex A of the triangle
85102
* @param B Vertex B of the triangle
@@ -99,6 +116,11 @@ PointInTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrix
99116
* @brief Obtain the point in the tetrahedron ABCD closest to the point P. The order of ABCD
100117
* must be such that all faces ABC, ACD, ADB and BDC are oriented with outwards pointing normals
101118
* when viewed from outside the tetrahedron.
119+
* @tparam TMatrixP Query point matrix type
120+
* @tparam TMatrixA Vertex A of the tetrahedron matrix type
121+
* @tparam TMatrixB Vertex B of the tetrahedron matrix type
122+
* @tparam TMatrixC Vertex C of the tetrahedron matrix type
123+
* @tparam TMatrixD Vertex D of the tetrahedron matrix type
102124
* @param P Query point
103125
* @param A Vertex A of the tetrahedron
104126
* @param B Vertex B of the tetrahedron

‎source/pbat/geometry/DistanceQueries.h‎

Lines changed: 105 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,13 @@
1+
/**
2+
* @file DistanceQueries.h
3+
* @author Quoc-Minh Ton-That ([email protected])
4+
* @brief This file contains functions to answer distance queries.
5+
* @date 2025-02-12
6+
*
7+
* @copyright Copyright (c) 2025
8+
*
9+
*/
10+
111
#ifndef PBAT_GEOMETRY_DISTANCEQUERIES_H
212
#define PBAT_GEOMETRY_DISTANCEQUERIES_H
313

@@ -10,143 +20,169 @@
1020

1121
namespace pbat {
1222
namespace geometry {
23+
/**
24+
* @brief This namespace contains functions to answer distance queries.
25+
*/
1326
namespace DistanceQueries {
1427

1528
namespace mini = math::linalg::mini;
1629

1730
/**
1831
* @brief Obtain squared distance between 2 axis-aligned bounding boxes
32+
* @tparam TMatrixL1 1st AABB's lower corner matrix type
33+
* @tparam TMatrixU1 1st AABB's upper corner matrix type
34+
* @tparam TMatrixL2 2nd AABB's lower corner matrix type
35+
* @tparam TMatrixU2 2nd AABB's upper corner matrix type
1936
* @param L1 1st AABB's lower corner
2037
* @param U1 1st AABB's upper corner
2138
* @param L2 2nd AABB's lower corner
2239
* @param U2 2nd AABB's upper corner
23-
* @return
40+
* @return Squared distance between the 2 AABBs
2441
*/
2542
template <
2643
mini::CMatrix TMatrixL1,
2744
mini::CMatrix TMatrixU1,
2845
mini::CMatrix TMatrixL2,
2946
mini::CMatrix TMatrixU2>
30-
PBAT_HOST_DEVICE typename TMatrixL1::ScalarType AxisAlignedBoundingBoxes(
47+
PBAT_HOST_DEVICE auto AxisAlignedBoundingBoxes(
3148
TMatrixL1 const& L1,
3249
TMatrixU1 const& U1,
3350
TMatrixL2 const& L2,
34-
TMatrixU2 const& U2);
51+
TMatrixU2 const& U2) -> typename TMatrixL1::ScalarType;
3552

3653
/**
3754
* @brief Obtain squared distance between point P and axis-aligned box (L,U)
38-
* @tparam TMatrixP
39-
* @tparam TMatrixL
40-
* @tparam TMatrixU
41-
* @param P
42-
* @param L
43-
* @param U
44-
* @return
55+
* @tparam TMatrixP Point matrix type
56+
* @tparam TMatrixL Lower corner matrix type
57+
* @tparam TMatrixU Upper corner matrix type
58+
* @param P Point
59+
* @param L Lower corner of the box
60+
* @param U Upper corner of the box
61+
* @return Squared distance between point and box
4562
*/
4663
template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
47-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType
48-
PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U);
64+
PBAT_HOST_DEVICE auto
65+
PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U) ->
66+
typename TMatrixP::ScalarType;
4967

5068
/**
5169
* @brief Obtain squared distance between point P and triangle ABC
52-
* @param P
53-
* @param A
54-
* @param B
55-
* @param C
56-
* @return
70+
* @tparam TMatrixP Point matrix type
71+
* @tparam TMatrixA Vertex A matrix type
72+
* @tparam TMatrixB Vertex B matrix type
73+
* @tparam TMatrixC Vertex C matrix type
74+
* @param P Point
75+
* @param A Vertex A of the triangle
76+
* @param B Vertex B of the triangle
77+
* @param C Vertex C of the triangle
78+
* @return Squared distance between point and triangle
5779
*/
5880
template <
5981
mini::CMatrix TMatrixP,
6082
mini::CMatrix TMatrixA,
6183
mini::CMatrix TMatrixB,
6284
mini::CMatrix TMatrixC>
63-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType
64-
PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C);
85+
PBAT_HOST_DEVICE auto
86+
PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C) ->
87+
typename TMatrixP::ScalarType;
6588

6689
/**
6790
* @brief Obtain squared distance between point P and tetrahedron ABCD
68-
* @tparam TMatrixP
69-
* @tparam TMatrixA
70-
* @tparam TMatrixB
71-
* @tparam TMatrixC
72-
* @tparam TMatrixD
73-
* @param P
74-
* @param A
75-
* @param B
76-
* @param C
77-
* @param D
78-
* @return
91+
* @tparam TMatrixP Point matrix type
92+
* @tparam TMatrixA Vertex A matrix type
93+
* @tparam TMatrixB Vertex B matrix type
94+
* @tparam TMatrixC Vertex C matrix type
95+
* @tparam TMatrixD Vertex D matrix type
96+
* @param P Point
97+
* @param A Vertex A of the tetrahedron
98+
* @param B Vertex B of the tetrahedron
99+
* @param C Vertex C of the tetrahedron
100+
* @param D Vertex D of the tetrahedron
101+
* @return Squared distance between point and tetrahedron
79102
*/
80103
template <
81104
mini::CMatrix TMatrixP,
82105
mini::CMatrix TMatrixA,
83106
mini::CMatrix TMatrixB,
84107
mini::CMatrix TMatrixC,
85108
mini::CMatrix TMatrixD>
86-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType PointTetrahedron(
109+
PBAT_HOST_DEVICE auto PointTetrahedron(
87110
TMatrixP const& P,
88111
TMatrixA const& A,
89112
TMatrixB const& B,
90113
TMatrixC const& C,
91-
TMatrixD const& D);
114+
TMatrixD const& D) -> typename TMatrixP::ScalarType;
92115

93116
/**
94117
* @brief Obtains the signed distance of X w.r.t. plane (P,n)
95-
* @param X
96-
* @param P
97-
* @param n
98-
* @return
118+
* @tparam TMatrixX Query point matrix type
119+
* @tparam TMatrixP Point on the plane matrix type
120+
* @tparam TMatrixN Normal of the plane matrix type
121+
* @param X Query point
122+
* @param P Point on the plane
123+
* @param n Normal of the plane
124+
* @return Signed distance of X w.r.t. plane (P,n)
99125
*/
100126
template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixN>
101-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType
102-
PointPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n);
127+
PBAT_HOST_DEVICE auto PointPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n) ->
128+
typename TMatrixX::ScalarType;
103129

104130
/**
105131
* @brief Obtains the signed distance of X w.r.t. plane spanned by ABC
106-
* @param X
107-
* @param P
108-
* @param n
109-
* @return
132+
* @tparam TMatrixX Query point matrix type
133+
* @tparam TMatrixA Vertex A of the triangle in plane matrix type
134+
* @tparam TMatrixB Vertex B of the triangle in plane matrix type
135+
* @tparam TMatrixC Vertex C of the triangle in plane matrix type
136+
* @param X Query point
137+
* @param A Vertex A of the triangle in plane
138+
* @param B Vertex B of the triangle in plane
139+
* @param C Vertex C of the triangle in plane
140+
* @return Signed distance of X w.r.t. plane spanned by ABC
110141
*/
111142
template <
112143
mini::CMatrix TMatrixX,
113144
mini::CMatrix TMatrixA,
114145
mini::CMatrix TMatrixB,
115146
mini::CMatrix TMatrixC>
116-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType
117-
PointPlane(TMatrixX const& X, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C);
147+
PBAT_HOST_DEVICE auto
148+
PointPlane(TMatrixX const& X, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C) ->
149+
typename TMatrixX::ScalarType;
118150

119151
/**
120152
* @brief Obtains the squared distance between sphere (X,R) and triangle ABC.
121-
* @param X
122-
* @param R
123-
* @param A
124-
* @param B
125-
* @param C
126-
* @return
153+
* @tparam TMatrixX Sphere center matrix type
154+
* @tparam TMatrixA Triangle vertex A matrix type
155+
* @tparam TMatrixB Triangle vertex B matrix type
156+
* @tparam TMatrixC Triangle vertex C matrix type
157+
* @param X Sphere center
158+
* @param R Sphere radius
159+
* @param A Vertex A of the triangle
160+
* @param B Vertex B of the triangle
161+
* @param C Vertex C of the triangle
162+
* @return Squared distance between sphere and triangle
127163
*/
128164
template <
129165
mini::CMatrix TMatrixX,
130166
mini::CMatrix TMatrixA,
131167
mini::CMatrix TMatrixB,
132168
mini::CMatrix TMatrixC>
133-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType SphereTriangle(
169+
PBAT_HOST_DEVICE auto SphereTriangle(
134170
TMatrixX const& X,
135171
typename TMatrixX::ScalarType R,
136172
TMatrixA const& A,
137173
TMatrixB const& B,
138-
TMatrixC const& C);
174+
TMatrixC const& C) -> typename TMatrixX::ScalarType;
139175

140176
template <
141177
mini::CMatrix TMatrixL1,
142178
mini::CMatrix TMatrixU1,
143179
mini::CMatrix TMatrixL2,
144180
mini::CMatrix TMatrixU2>
145-
PBAT_HOST_DEVICE typename TMatrixL1::ScalarType AxisAlignedBoundingBoxes(
181+
PBAT_HOST_DEVICE auto AxisAlignedBoundingBoxes(
146182
TMatrixL1 const& L1,
147183
TMatrixU1 const& U1,
148184
TMatrixL2 const& L2,
149-
TMatrixU2 const& U2)
185+
TMatrixU2 const& U2) -> typename TMatrixL1::ScalarType
150186
{
151187
using ScalarType = typename TMatrixL1::ScalarType;
152188
auto constexpr kDims = TMatrixL1::kRows;
@@ -159,8 +195,9 @@ PBAT_HOST_DEVICE typename TMatrixL1::ScalarType AxisAlignedBoundingBoxes(
159195
}
160196

161197
template <mini::CMatrix TMatrixP, mini::CMatrix TMatrixL, mini::CMatrix TMatrixU>
162-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType
163-
PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U)
198+
PBAT_HOST_DEVICE auto
199+
PointAxisAlignedBoundingBox(TMatrixP const& P, TMatrixL const& L, TMatrixU const& U) ->
200+
typename TMatrixP::ScalarType
164201
{
165202
// If point is inside AABB, then distance is 0.
166203
bool const bIsInsideBox = OverlapQueries::PointAxisAlignedBoundingBox(P, L, U);
@@ -176,8 +213,9 @@ template <
176213
mini::CMatrix TMatrixA,
177214
mini::CMatrix TMatrixB,
178215
mini::CMatrix TMatrixC>
179-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType
180-
PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
216+
PBAT_HOST_DEVICE auto
217+
PointTriangle(TMatrixP const& P, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C) ->
218+
typename TMatrixP::ScalarType
181219
{
182220
auto const PP = ClosestPointQueries::PointInTriangle(P, A, B, C);
183221
return SquaredNorm(P - PP);
@@ -189,12 +227,12 @@ template <
189227
mini::CMatrix TMatrixB,
190228
mini::CMatrix TMatrixC,
191229
mini::CMatrix TMatrixD>
192-
PBAT_HOST_DEVICE typename TMatrixP::ScalarType PointTetrahedron(
230+
PBAT_HOST_DEVICE auto PointTetrahedron(
193231
TMatrixP const& P,
194232
TMatrixA const& A,
195233
TMatrixB const& B,
196234
TMatrixC const& C,
197-
TMatrixD const& D)
235+
TMatrixD const& D) -> typename TMatrixP::ScalarType
198236
{
199237
bool const bPointInTetrahedron = OverlapQueries::PointTetrahedron3D(P, A, B, C, D);
200238
if (bPointInTetrahedron)
@@ -211,8 +249,8 @@ PBAT_HOST_DEVICE typename TMatrixP::ScalarType PointTetrahedron(
211249
}
212250

213251
template <mini::CMatrix TMatrixX, mini::CMatrix TMatrixP, mini::CMatrix TMatrixN>
214-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType
215-
PointPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n)
252+
PBAT_HOST_DEVICE auto PointPlane(TMatrixX const& X, TMatrixP const& P, TMatrixN const& n) ->
253+
typename TMatrixX::ScalarType
216254
{
217255
return Dot(X - P, n);
218256
}
@@ -222,8 +260,9 @@ template <
222260
mini::CMatrix TMatrixA,
223261
mini::CMatrix TMatrixB,
224262
mini::CMatrix TMatrixC>
225-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType
226-
PointPlane(TMatrixX const& X, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C)
263+
PBAT_HOST_DEVICE auto
264+
PointPlane(TMatrixX const& X, TMatrixA const& A, TMatrixB const& B, TMatrixC const& C) ->
265+
typename TMatrixX::ScalarType
227266
{
228267
auto const n = Cross(B - A, C - A);
229268
return PointPlane(X, A, n);
@@ -234,12 +273,12 @@ template <
234273
mini::CMatrix TMatrixA,
235274
mini::CMatrix TMatrixB,
236275
mini::CMatrix TMatrixC>
237-
PBAT_HOST_DEVICE typename TMatrixX::ScalarType SphereTriangle(
276+
PBAT_HOST_DEVICE auto SphereTriangle(
238277
TMatrixX const& X,
239278
typename TMatrixX::ScalarType R,
240279
TMatrixA const& A,
241280
TMatrixB const& B,
242-
TMatrixC const& C)
281+
TMatrixC const& C) -> typename TMatrixX::ScalarType
243282
{
244283
auto const sd2c = PointTriangle(X, A, B, C);
245284
return sd2c - R * R;

0 commit comments

Comments
 (0)