Skip to content

Commit d34ad70

Browse files
jwnimmer-trijwnimmer-tri
committed
[geometry] Move CalcBarycentric to be non-inline
1 parent 7970bd1 commit d34ad70

File tree

8 files changed

+146
-99
lines changed

8 files changed

+146
-99
lines changed

‎bindings/generated_docstrings/geometry_proximity.h‎

Lines changed: 17 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -485,7 +485,10 @@ Parameter ``p_MQ``:
485485
486486
Raises:
487487
RuntimeError if the field does not have gradients defined *and*
488-
the MeshType doesn't support Barycentric coordinates.)""";
488+
the MeshType doesn't support Barycentric coordinates.
489+
490+
Template parameter ``C``:
491+
must be either ``double`` or ``AutoDiffXd``.)""";
489492
} EvaluateCartesian;
490493
// Symbol: drake::geometry::MeshFieldLinear::EvaluateGradient
491494
struct /* EvaluateGradient */ {
@@ -871,7 +874,10 @@ elements (three or more sides).)""";
871874
R"""(See TriangleSurfaceMesh::CalcBaryCentric(). This implementation is
872875
provided to maintain compatibility with MeshFieldLinear. However, it
873876
only throws. PolygonSurfaceMesh does not support barycentric
874-
coordinates.)""";
877+
coordinates.
878+
879+
Template parameter ``C``:
880+
must be either ``double`` or ``AutoDiffXd``.)""";
875881
} CalcBarycentric;
876882
// Symbol: drake::geometry::PolygonSurfaceMesh::CalcBoundingBox
877883
struct /* CalcBoundingBox */ {
@@ -1292,7 +1298,7 @@ Parameter ``t``:
12921298
The index of a triangle.
12931299
12941300
Returns ``b_Q``:
1295-
' The barycentric coordinates of Q' (projection of Q onto `t`'s
1301+
' The barycentric coordinates of Q' (projection of Q onto ``t`'s
12961302
plane) relative to triangle t.
12971303
12981304
Note:
@@ -1301,7 +1307,10 @@ Returns ``b_Q``:
13011307
negative.
13021308
13031309
Precondition:
1304-
t ∈ {0, 1, 2,..., num_triangles()-1}.)""";
1310+
t ∈ {0, 1, 2,..., num_triangles()-1}.
1311+
1312+
Template parameter ``C``:
1313+
must be either `double`` or ``AutoDiffXd``.)""";
13051314
} CalcBarycentric;
13061315
// Symbol: drake::geometry::TriangleSurfaceMesh::CalcBoundingBox
13071316
struct /* CalcBoundingBox */ {
@@ -1616,7 +1625,10 @@ Parameter ``e``:
16161625
Note:
16171626
If p_MQ is outside the tetrahedral element, the barycentric
16181627
coordinates (b₀, b₁, b₂, b₃) still satisfy b₀ + b₁ + b₂ + b₃ = 1;
1619-
however, some bᵢ will be negative.)""";
1628+
however, some bᵢ will be negative.
1629+
1630+
Template parameter ``C``:
1631+
must be either ``double`` or ``AutoDiffXd``.)""";
16201632
} CalcBarycentric;
16211633
// Symbol: drake::geometry::VolumeMesh::CalcGradientVectorOfLinearField
16221634
struct /* CalcGradientVectorOfLinearField */ {

‎geometry/proximity/mesh_field_linear.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -330,7 +330,7 @@ class MeshFieldLinear {
330330
coordinates. M is the frame of the mesh.
331331
@throws std::exception if the field does not have gradients defined _and_ the
332332
MeshType doesn't support Barycentric coordinates.
333-
*/
333+
@tparam C must be either `double` or `AutoDiffXd`. */
334334
template <typename C>
335335
promoted_numerical_t<C, T> EvaluateCartesian(int e,
336336
const Vector3<C>& p_MQ) const {

‎geometry/proximity/polygon_surface_mesh.cc‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,18 @@ void PolygonSurfaceMesh<T>::ReverseFaceWinding() {
5252
}
5353
}
5454

55+
template <typename T>
56+
template <typename C>
57+
typename PolygonSurfaceMesh<T>::template Barycentric<promoted_numerical_t<T, C>>
58+
PolygonSurfaceMesh<T>::CalcBarycentric(const Vector3<C>& p_MQ, int p) const
59+
requires scalar_predicate<C>::is_bool
60+
{
61+
unused(p_MQ, p);
62+
throw std::runtime_error(
63+
"PolygonSurfaceMesh::CalcBarycentric(): PolygonSurfaceMesh does not have "
64+
"barycentric coordinates.");
65+
}
66+
5567
template <typename T>
5668
std::pair<Vector3<T>, Vector3<T>> PolygonSurfaceMesh<T>::CalcBoundingBox()
5769
const {
@@ -243,5 +255,8 @@ void PolygonSurfaceMesh<T>::SetAllPositions(
243255
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
244256
class PolygonSurfaceMesh);
245257

258+
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
259+
(&PolygonSurfaceMesh<T>::template CalcBarycentric<U>));
260+
246261
} // namespace geometry
247262
} // namespace drake

‎geometry/proximity/polygon_surface_mesh.h‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -233,15 +233,12 @@ class PolygonSurfaceMesh {
233233

234234
/** See TriangleSurfaceMesh::CalcBaryCentric(). This implementation is
235235
provided to maintain compatibility with MeshFieldLinear. However, it only
236-
throws. %PolygonSurfaceMesh does not support barycentric coordinates. */
236+
throws. %PolygonSurfaceMesh does not support barycentric coordinates.
237+
@tparam C must be either `double` or `AutoDiffXd`. */
237238
template <typename C>
238239
Barycentric<promoted_numerical_t<T, C>> CalcBarycentric(
239-
const Vector3<C>& p_MQ, int p) const {
240-
unused(p_MQ, p);
241-
throw std::runtime_error(
242-
"PolygonSurfaceMesh::CalcBarycentric(): PolygonSurfaceMesh does not "
243-
"have barycentric coordinates.");
244-
}
240+
const Vector3<C>& p_MQ, int p) const
241+
requires scalar_predicate<C>::is_bool;
245242

246243
// TODO(DamrongGuoy): Consider using an oriented bounding box in obb.h.
247244
// Currently we have a problem that PolygonSurfaceMesh and its vertices are

‎geometry/proximity/triangle_surface_mesh.cc‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,69 @@ void TriangleSurfaceMesh<T>::ReverseFaceWinding() {
1515
}
1616
}
1717

18+
template <typename T>
19+
template <typename C>
20+
typename TriangleSurfaceMesh<T>::template Barycentric<
21+
promoted_numerical_t<T, C>>
22+
TriangleSurfaceMesh<T>::CalcBarycentric(const Vector3<C>& p_MQ, int t) const
23+
requires scalar_predicate<C>::is_bool
24+
{
25+
const Vector3<T>& v0 = vertex(element(t).vertex(0));
26+
const Vector3<T>& v1 = vertex(element(t).vertex(1));
27+
const Vector3<T>& v2 = vertex(element(t).vertex(2));
28+
// Translate the triangle to the origin to simplify calculations;
29+
// barycentric coordinates stay the same.
30+
// u⃗i = v⃗i - v0
31+
// p_MR = p_MQ - v0
32+
//
33+
// Consider R' on the spanning plane through the origin, u1, u2:
34+
// R' = b₀*u0 + b₁*u1 + b₂*u2
35+
// = 0 + b₁*u1 + b₂*u2
36+
// = b₁*u1 + b₂*u2
37+
//
38+
// Solve for b₁, b₂ that give R' "closest" to R in the least square sense:
39+
//
40+
// | ||b1|
41+
// |u⃗1 u⃗2||b2| ~ R'
42+
// | |
43+
//
44+
// return Barycentric (1-b₁-b₂, b₁, b₂)
45+
//
46+
using ReturnType = promoted_numerical_t<T, C>;
47+
Eigen::Matrix<ReturnType, 3, 2> A;
48+
A.col(0) << v1 - v0;
49+
A.col(1) << v2 - v0;
50+
Vector2<ReturnType> solution = A.colPivHouseholderQr().solve(p_MQ - v0);
51+
52+
const ReturnType& b1 = solution(0);
53+
const ReturnType& b2 = solution(1);
54+
const ReturnType b0 = T(1.) - b1 - b2;
55+
return {b0, b1, b2};
56+
}
57+
// TODO(DamrongGuoy): Investigate alternative calculation suggested by
58+
// Alejandro Castro:
59+
// 1. Starting with the same ui and p_MR.
60+
// 2. Calculate the unit normal vector n to the spanning plane S through
61+
// the origin, u1, and u2.
62+
// n = u1.cross(u2).normalize().
63+
// 3. Project p_MR to p_MR' on the plane S,
64+
// p_MR' = p_MR - (p_MR.dot(n))*n
65+
//
66+
// Now we have p_MR' = b₀*u⃗0 + b₁*u⃗1 + b₂*u⃗2 by barycentric coordinates.
67+
// = 0 + b₁*u1 + b₂*u2
68+
//
69+
// 5. Solve for b₁ and b₂.
70+
// (b₁*u1 + b₂*u2).dot(u1) = p_MR'.dot(u1)
71+
// (b₁*u1 + b₂*u2).dot(u2) = p_MR'.dot(u2)
72+
// Therefore, the 2x2 system:
73+
// |u1.dot(u1) u2.dot(u1)||b1| = |p_MR'.dot(u1)|
74+
// |u1.dot(u2) u2.dot(u2)||b2| |p_MR'.dot(u2)|
75+
//
76+
// 6. return Barycentric(1-b₁-b₂, b₁, b₂)
77+
//
78+
// Optimization: save n, and the inverse of matrix |uᵢ.dot(uⱼ)| for later.
79+
//
80+
1881
template <typename T>
1982
void TriangleSurfaceMesh<T>::SetAllPositions(
2083
const Eigen::Ref<const VectorX<T>>& p_MVs) {
@@ -35,5 +98,8 @@ void TriangleSurfaceMesh<T>::SetAllPositions(
3598
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
3699
class TriangleSurfaceMesh);
37100

101+
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
102+
(&TriangleSurfaceMesh<T>::template CalcBarycentric<U>));
103+
38104
} // namespace geometry
39105
} // namespace drake

‎geometry/proximity/triangle_surface_mesh.h‎

Lines changed: 3 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -287,65 +287,11 @@ class TriangleSurfaceMesh {
287287
(b₀, b₁, b₂) still satisfy b₀ + b₁ + b₂ = 1; however, some bᵢ will be
288288
negative.
289289
@pre t ∈ {0, 1, 2,..., num_triangles()-1}.
290-
*/
290+
@tparam C must be either `double` or `AutoDiffXd`. */
291291
template <typename C>
292292
Barycentric<promoted_numerical_t<T, C>> CalcBarycentric(
293-
const Vector3<C>& p_MQ, int t) const {
294-
const Vector3<T>& v0 = vertex(element(t).vertex(0));
295-
const Vector3<T>& v1 = vertex(element(t).vertex(1));
296-
const Vector3<T>& v2 = vertex(element(t).vertex(2));
297-
// Translate the triangle to the origin to simplify calculations;
298-
// barycentric coordinates stay the same.
299-
// u⃗i = v⃗i - v0
300-
// p_MR = p_MQ - v0
301-
//
302-
// Consider R' on the spanning plane through the origin, u1, u2:
303-
// R' = b₀*u0 + b₁*u1 + b₂*u2
304-
// = 0 + b₁*u1 + b₂*u2
305-
// = b₁*u1 + b₂*u2
306-
//
307-
// Solve for b₁, b₂ that give R' "closest" to R in the least square sense:
308-
//
309-
// | ||b1|
310-
// |u⃗1 u⃗2||b2| ~ R'
311-
// | |
312-
//
313-
// return Barycentric (1-b₁-b₂, b₁, b₂)
314-
//
315-
using ReturnType = promoted_numerical_t<T, C>;
316-
Eigen::Matrix<ReturnType, 3, 2> A;
317-
A.col(0) << v1 - v0;
318-
A.col(1) << v2 - v0;
319-
Vector2<ReturnType> solution = A.colPivHouseholderQr().solve(p_MQ - v0);
320-
321-
const ReturnType& b1 = solution(0);
322-
const ReturnType& b2 = solution(1);
323-
const ReturnType b0 = T(1.) - b1 - b2;
324-
return {b0, b1, b2};
325-
}
326-
// TODO(DamrongGuoy): Investigate alternative calculation suggested by
327-
// Alejandro Castro:
328-
// 1. Starting with the same ui and p_MR.
329-
// 2. Calculate the unit normal vector n to the spanning plane S through
330-
// the origin, u1, and u2.
331-
// n = u1.cross(u2).normalize().
332-
// 3. Project p_MR to p_MR' on the plane S,
333-
// p_MR' = p_MR - (p_MR.dot(n))*n
334-
//
335-
// Now we have p_MR' = b₀*u⃗0 + b₁*u⃗1 + b₂*u⃗2 by barycentric coordinates.
336-
// = 0 + b₁*u1 + b₂*u2
337-
//
338-
// 5. Solve for b₁ and b₂.
339-
// (b₁*u1 + b₂*u2).dot(u1) = p_MR'.dot(u1)
340-
// (b₁*u1 + b₂*u2).dot(u2) = p_MR'.dot(u2)
341-
// Therefore, the 2x2 system:
342-
// |u1.dot(u1) u2.dot(u1)||b1| = |p_MR'.dot(u1)|
343-
// |u1.dot(u2) u2.dot(u2)||b2| |p_MR'.dot(u2)|
344-
//
345-
// 6. return Barycentric(1-b₁-b₂, b₁, b₂)
346-
//
347-
// Optimization: save n, and the inverse of matrix |uᵢ.dot(uⱼ)| for later.
348-
//
293+
const Vector3<C>& p_MQ, int t) const
294+
requires scalar_predicate<C>::is_bool;
349295

350296
// TODO(DamrongGuoy): Consider using an oriented bounding box in obb.h.
351297
// Currently we have a problem that TriangleSurfaceMesh and its vertices are

‎geometry/proximity/volume_mesh.cc‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include "drake/geometry/proximity/volume_mesh.h"
22

33
#include "drake/common/default_scalars.h"
4+
#include "drake/math/linear_solve.h"
45

56
namespace drake {
67
namespace geometry {
@@ -15,6 +16,39 @@ VolumeMesh<T>::VolumeMesh(std::vector<VolumeElement>&& elements,
1516
ComputePositionDependentQuantities();
1617
}
1718

19+
template <typename T>
20+
template <typename C>
21+
typename VolumeMesh<T>::template Barycentric<promoted_numerical_t<T, C>>
22+
VolumeMesh<T>::CalcBarycentric(const Vector3<C>& p_MQ, int e) const
23+
requires scalar_predicate<C>::is_bool
24+
{
25+
// We have two conditions to satisfy.
26+
// 1. b₀ + b₁ + b₂ + b₃ = 1
27+
// 2. b₀*v0 + b₁*v1 + b₂*v2 + b₃*v3 = p_M.
28+
// Together they create this 4x4 linear system:
29+
//
30+
// | 1 1 1 1 ||b₀| | 1 |
31+
// | | | | | ||b₁| = | | |
32+
// | v0 v1 v2 v3||b₂| |p_M|
33+
// | | | | | ||b₃| | | |
34+
//
35+
// q = p_M - v0 = b₀*u0 + b₁*u1 + b₂*u2 + b₃*u3
36+
// = 0 + b₁*u1 + b₂*u2 + b₃*u3
37+
using ReturnType = promoted_numerical_t<T, C>;
38+
Matrix4<ReturnType> A;
39+
for (int i = 0; i < 4; ++i) {
40+
A.col(i) << ReturnType(1.0), vertex(element(e).vertex(i));
41+
}
42+
Vector4<ReturnType> b;
43+
b << ReturnType(1.0), p_MQ;
44+
const math::LinearSolver<Eigen::PartialPivLU, Matrix4<ReturnType>> A_lu(A);
45+
const Vector4<ReturnType> b_Q = A_lu.Solve(b);
46+
// TODO(DamrongGuoy): Save the inverse of the matrix instead of
47+
// calculating it on the fly. We can reduce to 3x3 system too. See
48+
// issue #11653.
49+
return b_Q;
50+
}
51+
1852
template <typename T>
1953
void VolumeMesh<T>::TransformVertices(
2054
const math::RigidTransform<T>& transform) {
@@ -127,5 +161,8 @@ bool VolumeMesh<T>::Equal(const VolumeMesh<T>& mesh,
127161
DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
128162
class VolumeMesh);
129163

164+
DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
165+
(&VolumeMesh<T>::template CalcBarycentric<U>));
166+
130167
} // namespace geometry
131168
} // namespace drake

‎geometry/proximity/volume_mesh.h‎

Lines changed: 3 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@
1313
#include "drake/common/drake_copyable.h"
1414
#include "drake/common/eigen_types.h"
1515
#include "drake/geometry/proximity/mesh_traits.h"
16-
#include "drake/math/linear_solve.h"
1716
#include "drake/math/rigid_transform.h"
1817

1918
namespace drake {
@@ -241,36 +240,11 @@ class VolumeMesh {
241240
@note If p_MQ is outside the tetrahedral element, the barycentric
242241
coordinates (b₀, b₁, b₂, b₃) still satisfy b₀ + b₁ + b₂ + b₃ = 1;
243242
however, some bᵢ will be negative.
244-
*/
243+
@tparam C must be either `double` or `AutoDiffXd`. */
245244
template <typename C>
246245
Barycentric<promoted_numerical_t<T, C>> CalcBarycentric(
247-
const Vector3<C>& p_MQ, int e) const {
248-
// We have two conditions to satisfy.
249-
// 1. b₀ + b₁ + b₂ + b₃ = 1
250-
// 2. b₀*v0 + b₁*v1 + b₂*v2 + b₃*v3 = p_M.
251-
// Together they create this 4x4 linear system:
252-
//
253-
// | 1 1 1 1 ||b₀| | 1 |
254-
// | | | | | ||b₁| = | | |
255-
// | v0 v1 v2 v3||b₂| |p_M|
256-
// | | | | | ||b₃| | | |
257-
//
258-
// q = p_M - v0 = b₀*u0 + b₁*u1 + b₂*u2 + b₃*u3
259-
// = 0 + b₁*u1 + b₂*u2 + b₃*u3
260-
using ReturnType = promoted_numerical_t<T, C>;
261-
Matrix4<ReturnType> A;
262-
for (int i = 0; i < 4; ++i) {
263-
A.col(i) << ReturnType(1.0), vertex(element(e).vertex(i));
264-
}
265-
Vector4<ReturnType> b;
266-
b << ReturnType(1.0), p_MQ;
267-
const math::LinearSolver<Eigen::PartialPivLU, Matrix4<ReturnType>> A_lu(A);
268-
const Vector4<ReturnType> b_Q = A_lu.Solve(b);
269-
// TODO(DamrongGuoy): Save the inverse of the matrix instead of
270-
// calculating it on the fly. We can reduce to 3x3 system too. See
271-
// issue #11653.
272-
return b_Q;
273-
}
246+
const Vector3<C>& p_MQ, int e) const
247+
requires scalar_predicate<C>::is_bool;
274248

275249
/** Checks to see whether the given VolumeMesh object is equal via deep
276250
comparison (up to a tolerance). NaNs are treated as not equal as per the IEEE

0 commit comments

Comments
 (0)