Skip to content

Commit 219f10b

Browse files
zferguszfergus
committed
Add compute_coefficients to CollisionStencil
1 parent bd4fa95 commit 219f10b

File tree

17 files changed

+324
-12
lines changed

17 files changed

+324
-12
lines changed

‎python/src/utils/interval.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,9 @@
44
#include <ipc/utils/interval.hpp>
55

66
namespace py = pybind11;
7+
#ifdef IPC_TOOLKIT_WITH_FILIB
78
using namespace filib;
9+
#endif
810

911
// ============================================================================
1012

‎src/ipc/barrier/barrier.hpp‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -141,9 +141,9 @@ class ClampedLogBarrier : public Barrier {
141141
// ============================================================================
142142

143143
/// @brief Normalized barrier function from [Li et al. 2023].
144-
class NormalizedClampedLogBarrier : public ClampedLogBarrier {
144+
template <typename BarrierT> class NormalizedBarrier : public BarrierT {
145145
public:
146-
NormalizedClampedLogBarrier() = default;
146+
NormalizedBarrier() = default;
147147

148148
/// @brief Function that grows to infinity as d approaches 0 from the right.
149149
///
@@ -157,7 +157,7 @@ class NormalizedClampedLogBarrier : public ClampedLogBarrier {
157157
/// @return The value of the barrier function at d.
158158
double operator()(const double d, const double dhat) const override
159159
{
160-
return ClampedLogBarrier::operator()(d / dhat, 1.0);
160+
return BarrierT::operator()(d / dhat, 1.0);
161161
}
162162

163163
/// @brief Derivative of the barrier function.
@@ -173,7 +173,7 @@ class NormalizedClampedLogBarrier : public ClampedLogBarrier {
173173
/// @return The derivative of the barrier wrt d.
174174
double first_derivative(const double d, const double dhat) const override
175175
{
176-
return ClampedLogBarrier::first_derivative(d / dhat, 1.0) / dhat;
176+
return BarrierT::first_derivative(d / dhat, 1.0) / dhat;
177177
}
178178

179179
/// @brief Second derivative of the barrier function.
@@ -188,8 +188,7 @@ class NormalizedClampedLogBarrier : public ClampedLogBarrier {
188188
/// @return The second derivative of the barrier wrt d.
189189
double second_derivative(const double d, const double dhat) const override
190190
{
191-
return ClampedLogBarrier::second_derivative(d / dhat, 1.0)
192-
/ (dhat * dhat);
191+
return BarrierT::second_derivative(d / dhat, 1.0) / (dhat * dhat);
193192
}
194193

195194
/// @brief Get the units of the barrier function.
@@ -198,6 +197,8 @@ class NormalizedClampedLogBarrier : public ClampedLogBarrier {
198197
double units(const double dhat) const override { return 1.0; }
199198
};
200199

200+
using NormalizedClampedLogBarrier = NormalizedBarrier<ClampedLogBarrier>;
201+
201202
// ============================================================================
202203
// Quadratic log barrier functions from [Huang et al. 2024]
203204
// ============================================================================

‎src/ipc/candidates/collision_stencil.hpp‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,19 @@ class CollisionStencil {
119119
return compute_distance_hessian(dof(vertices, edges, faces));
120120
}
121121

122+
/// @brief Compute the coefficients of the stencil s.t. d(x) = ‖∑ cᵢ xᵢ‖².
123+
/// @param vertices Collision mesh vertices
124+
/// @param edges Collision mesh edges
125+
/// @param faces Collision mesh faces
126+
/// @return Coefficients of the stencil.
127+
VectorMax4d compute_coefficients(
128+
const Eigen::MatrixXd& vertices,
129+
const Eigen::MatrixXi& edges,
130+
const Eigen::MatrixXi& faces) const
131+
{
132+
return compute_coefficients(dof(vertices, edges, faces));
133+
}
134+
122135
// ----------------------------------------------------------------------
123136
// NOTE: The following functions take stencil vertices as output by dof()
124137
// ----------------------------------------------------------------------
@@ -142,6 +155,12 @@ class CollisionStencil {
142155
/// @return Distance Hessian of the stencil w.r.t. the stencil's vertex positions.
143156
virtual MatrixMax12d
144157
compute_distance_hessian(const VectorMax12d& positions) const = 0;
158+
159+
/// @brief Compute the coefficients of the stencil s.t. d(x) = ‖∑ cᵢ xᵢ‖².
160+
/// @param positions Stencil's vertex positions.
161+
/// @return Coefficients of the stencil.
162+
virtual VectorMax4d
163+
compute_coefficients(const VectorMax12d& positions) const = 0;
145164
};
146165

147166
} // namespace ipc

‎src/ipc/candidates/edge_edge.cpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include "edge_edge.hpp"
22

33
#include <ipc/distance/edge_edge.hpp>
4+
#include <ipc/tangent/closest_point.hpp>
45

56
namespace ipc {
67

@@ -36,6 +37,63 @@ EdgeEdgeCandidate::compute_distance_hessian(const VectorMax12d& positions) const
3637
positions.tail<3>(), known_dtype());
3738
}
3839

40+
VectorMax4d
41+
EdgeEdgeCandidate::compute_coefficients(const VectorMax12d& positions) const
42+
{
43+
assert(positions.size() == 12);
44+
const Eigen::Ref<const Eigen::Vector3d> ea0 = positions.head<3>();
45+
const Eigen::Ref<const Eigen::Vector3d> ea1 = positions.segment<3>(3);
46+
const Eigen::Ref<const Eigen::Vector3d> eb0 = positions.segment<3>(6);
47+
const Eigen::Ref<const Eigen::Vector3d> eb1 = positions.tail<3>();
48+
49+
// Project the point inside the triangle
50+
auto dtype = known_dtype();
51+
if (dtype == EdgeEdgeDistanceType::AUTO) {
52+
dtype = edge_edge_distance_type(ea0, ea1, eb0, eb1);
53+
}
54+
55+
VectorMax4d coeffs(4);
56+
switch (dtype) {
57+
case EdgeEdgeDistanceType::EA0_EB0:
58+
coeffs << 1.0, 0.0, -1.0, 0.0;
59+
break;
60+
case EdgeEdgeDistanceType::EA0_EB1:
61+
coeffs << 1.0, 0.0, 0.0, -1.0;
62+
break;
63+
case EdgeEdgeDistanceType::EA1_EB0:
64+
coeffs << 0.0, 1.0, -1.0, 0.0;
65+
break;
66+
case EdgeEdgeDistanceType::EA1_EB1:
67+
coeffs << 0.0, 1.0, 0.0, -1.0;
68+
break;
69+
case EdgeEdgeDistanceType::EA_EB0: {
70+
const double alpha = point_edge_closest_point(eb0, ea0, ea1);
71+
coeffs << 1.0 - alpha, alpha, -1.0, 0;
72+
} break;
73+
case EdgeEdgeDistanceType::EA_EB1: {
74+
const double alpha = point_edge_closest_point(eb1, ea0, ea1);
75+
coeffs << 1.0 - alpha, alpha, 0, -1.0;
76+
} break;
77+
case EdgeEdgeDistanceType::EA0_EB: {
78+
const double alpha = point_edge_closest_point(ea0, eb0, eb1);
79+
coeffs << 1.0, 0, -1.0 + alpha, -alpha;
80+
} break;
81+
case EdgeEdgeDistanceType::EA1_EB: {
82+
const double alpha = point_edge_closest_point(ea1, eb0, eb1);
83+
coeffs << 0, 1.0, -1.0 + alpha, -alpha;
84+
} break;
85+
case EdgeEdgeDistanceType::EA_EB: {
86+
const Eigen::Vector2d ab = edge_edge_closest_point(ea0, ea1, eb0, eb1);
87+
coeffs << 1.0 - ab[0], ab[0], -1.0 + ab[1], -ab[1];
88+
} break;
89+
default:
90+
assert(false);
91+
break;
92+
}
93+
94+
return coeffs;
95+
}
96+
3997
bool EdgeEdgeCandidate::ccd(
4098
const VectorMax12d& vertices_t0,
4199
const VectorMax12d& vertices_t1,

‎src/ipc/candidates/edge_edge.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ class EdgeEdgeCandidate : public ContinuousCollisionCandidate {
2626
edges(edge1_id, 0), edges(edge1_id, 1) } };
2727
}
2828

29+
using CollisionStencil::compute_coefficients;
2930
using CollisionStencil::compute_distance;
3031
using CollisionStencil::compute_distance_gradient;
3132
using CollisionStencil::compute_distance_hessian;
@@ -38,6 +39,9 @@ class EdgeEdgeCandidate : public ContinuousCollisionCandidate {
3839
MatrixMax12d
3940
compute_distance_hessian(const VectorMax12d& positions) const override;
4041

42+
VectorMax4d
43+
compute_coefficients(const VectorMax12d& positions) const override;
44+
4145
// ------------------------------------------------------------------------
4246
// ContinuousCollisionCandidate
4347

‎src/ipc/candidates/edge_vertex.cpp‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include "edge_vertex.hpp"
22

33
#include <ipc/distance/point_edge.hpp>
4+
#include <ipc/tangent/closest_point.hpp>
45

56
#include <iostream>
67

@@ -42,6 +43,40 @@ MatrixMax12d EdgeVertexCandidate::compute_distance_hessian(
4243
known_dtype());
4344
}
4445

46+
VectorMax4d
47+
EdgeVertexCandidate::compute_coefficients(const VectorMax12d& positions) const
48+
{
49+
assert(positions.size() == 6 || positions.size() == 9);
50+
const int dim = this->dim(positions.size());
51+
const Eigen::Ref<const VectorMax3d> p = positions.head(dim);
52+
const Eigen::Ref<const VectorMax3d> t0 = positions.segment(dim, dim);
53+
const Eigen::Ref<const VectorMax3d> t1 = positions.tail(dim);
54+
55+
auto dtype = known_dtype();
56+
if (dtype == PointEdgeDistanceType::AUTO) {
57+
dtype = point_edge_distance_type(p, t0, t1);
58+
}
59+
60+
VectorMax4d coeffs(3);
61+
switch (dtype) {
62+
case PointEdgeDistanceType::P_E0:
63+
coeffs << 1.0, -1.0, 0.0;
64+
break;
65+
case PointEdgeDistanceType::P_E1:
66+
coeffs << 1.0, 0.0, -1.0;
67+
break;
68+
case PointEdgeDistanceType::P_E: {
69+
const double alpha = point_edge_closest_point(p, t0, t1);
70+
coeffs << 1.0, -1.0 + alpha, -alpha;
71+
} break;
72+
default:
73+
assert(false);
74+
break;
75+
}
76+
77+
return coeffs;
78+
}
79+
4580
bool EdgeVertexCandidate::ccd(
4681
const VectorMax12d& vertices_t0,
4782
const VectorMax12d& vertices_t1,

‎src/ipc/candidates/edge_vertex.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ class EdgeVertexCandidate : public ContinuousCollisionCandidate {
2525
return { { vertex_id, edges(edge_id, 0), edges(edge_id, 1), -1 } };
2626
}
2727

28+
using CollisionStencil::compute_coefficients;
2829
using CollisionStencil::compute_distance;
2930
using CollisionStencil::compute_distance_gradient;
3031
using CollisionStencil::compute_distance_hessian;
@@ -37,6 +38,9 @@ class EdgeVertexCandidate : public ContinuousCollisionCandidate {
3738
MatrixMax12d
3839
compute_distance_hessian(const VectorMax12d& positions) const override;
3940

41+
VectorMax4d
42+
compute_coefficients(const VectorMax12d& positions) const override;
43+
4044
// ------------------------------------------------------------------------
4145
// ContinuousCollisionCandidate
4246

‎src/ipc/candidates/face_vertex.cpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#include "face_vertex.hpp"
22

33
#include <ipc/distance/point_triangle.hpp>
4+
#include <ipc/tangent/closest_point.hpp>
45

56
#include <iostream>
67

@@ -39,6 +40,56 @@ MatrixMax12d FaceVertexCandidate::compute_distance_hessian(
3940
positions.tail<3>(), known_dtype());
4041
}
4142

43+
VectorMax4d
44+
FaceVertexCandidate::compute_coefficients(const VectorMax12d& positions) const
45+
{
46+
assert(positions.size() == 12);
47+
const Eigen::Ref<const Eigen::Vector3d> p = positions.head<3>();
48+
const Eigen::Ref<const Eigen::Vector3d> t0 = positions.segment<3>(3);
49+
const Eigen::Ref<const Eigen::Vector3d> t1 = positions.segment<3>(6);
50+
const Eigen::Ref<const Eigen::Vector3d> t2 = positions.tail<3>();
51+
52+
// Project the point inside the triangle
53+
auto dtype = known_dtype();
54+
if (dtype == PointTriangleDistanceType::AUTO) {
55+
dtype = point_triangle_distance_type(p, t0, t1, t2);
56+
}
57+
58+
VectorMax4d coeffs(4);
59+
switch (dtype) {
60+
case PointTriangleDistanceType::P_T0:
61+
coeffs << 1.0, -1.0, 0.0, 0.0;
62+
break;
63+
case PointTriangleDistanceType::P_T1:
64+
coeffs << 1.0, 0.0, -1.0, 0.0;
65+
break;
66+
case PointTriangleDistanceType::P_T2:
67+
coeffs << 1.0, 0.0, 0.0, -1.0;
68+
break;
69+
case PointTriangleDistanceType::P_E0: {
70+
const double alpha = point_edge_closest_point(p, t0, t1);
71+
coeffs << 1.0, -alpha, -1 + alpha, 0.0;
72+
} break;
73+
case PointTriangleDistanceType::P_E1: {
74+
const double alpha = point_edge_closest_point(p, t1, t2);
75+
coeffs << 1.0, 0.0, -alpha, -1 + alpha;
76+
} break;
77+
case PointTriangleDistanceType::P_E2: {
78+
const double alpha = point_edge_closest_point(p, t2, t0);
79+
coeffs << 1.0, -1 + alpha, 0.0, -alpha;
80+
} break;
81+
case PointTriangleDistanceType::P_T: {
82+
const Eigen::Vector2d uv = point_triangle_closest_point(p, t0, t1, t2);
83+
coeffs << 1.0, -1.0 + uv[0] + uv[1], -uv[0], -uv[1];
84+
} break;
85+
default:
86+
assert(false);
87+
break;
88+
}
89+
90+
return coeffs;
91+
}
92+
4293
bool FaceVertexCandidate::ccd(
4394
const VectorMax12d& vertices_t0,
4495
const VectorMax12d& vertices_t1,

‎src/ipc/candidates/face_vertex.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ class FaceVertexCandidate : public ContinuousCollisionCandidate {
2626
faces(face_id, 2) } };
2727
}
2828

29+
using CollisionStencil::compute_coefficients;
2930
using CollisionStencil::compute_distance;
3031
using CollisionStencil::compute_distance_gradient;
3132
using CollisionStencil::compute_distance_hessian;
@@ -38,6 +39,9 @@ class FaceVertexCandidate : public ContinuousCollisionCandidate {
3839
MatrixMax12d
3940
compute_distance_hessian(const VectorMax12d& positions) const override;
4041

42+
VectorMax4d
43+
compute_coefficients(const VectorMax12d& positions) const override;
44+
4145
// ------------------------------------------------------------------------
4246
// ContinuousCollisionCandidate
4347

‎src/ipc/candidates/vertex_vertex.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,14 @@ MatrixMax12d VertexVertexCandidate::compute_distance_hessian(
3838
positions.head(dim), positions.tail(dim));
3939
}
4040

41+
VectorMax4d
42+
VertexVertexCandidate::compute_coefficients(const VectorMax12d& positions) const
43+
{
44+
VectorMax4d coeffs(2);
45+
coeffs << 1.0, -1.0;
46+
return coeffs;
47+
}
48+
4149
bool VertexVertexCandidate::ccd(
4250
const VectorMax12d& vertices_t0,
4351
const VectorMax12d& vertices_t1,

0 commit comments

Comments
 (0)