Skip to content

Commit dbb1ab4

Browse files
zfergusCopilot
zfergus
and
Copilot
authored
Add normal computation and Jacobian methods for collision candidates (#182)
* Implemented compute_normal and compute_normal_jacobian methods for various collision candidates including VertexVertex, EdgeVertex, EdgeEdge, FaceVertex, and PlaneVertex. * Added unit tests for normal computation in test_normals.cpp. * Updated CMakeLists.txt to include the new test file. * Refactor normal computation methods to use cross_product_matrix for clarity and consistency * Add concurrency settings to GitHub workflows for improved job management * Update Python version matrix to only use Python 3.13 for pull requests --------- Co-authored-by: Copilot <[email protected]>
1 parent 6895dda commit dbb1ab4

File tree

19 files changed

+538
-1
lines changed

19 files changed

+538
-1
lines changed

‎.github/workflows/continuous.yml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,10 @@ on:
1111
- 'tests/**'
1212
- 'CMakeLists.txt'
1313

14+
concurrency:
15+
group: ${{ github.workflow }}-${{ github.ref }}
16+
cancel-in-progress: ${{ github.ref != 'refs/heads/main' }}
17+
1418
env:
1519
CTEST_OUTPUT_ON_FAILURE: ON
1620
CTEST_PARALLEL_LEVEL: 2

‎.github/workflows/coverage.yml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,10 @@ on:
1212
- 'CMakeLists.txt'
1313
- 'codecov.yml'
1414

15+
concurrency:
16+
group: ${{ github.workflow }}-${{ github.ref }}
17+
cancel-in-progress: ${{ github.ref != 'refs/heads/main' }}
18+
1519
jobs:
1620
Coverage:
1721
name: Code Coverage

‎.github/workflows/cuda.yml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,10 @@ on:
1212
- 'python/**'
1313
- 'CMakeLists.txt'
1414

15+
concurrency:
16+
group: ${{ github.workflow }}-${{ github.ref }}
17+
cancel-in-progress: ${{ github.ref != 'refs/heads/main' }}
18+
1519
env:
1620
CTEST_OUTPUT_ON_FAILURE: ON
1721
CTEST_PARALLEL_LEVEL: 2

‎.github/workflows/python.yml‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,10 @@ on:
1515
- 'pyproject.toml'
1616
- 'setup.py'
1717

18+
concurrency:
19+
group: ${{ github.workflow }}-${{ github.ref }}
20+
cancel-in-progress: ${{ github.ref != 'refs/heads/main' }}
21+
1822
jobs:
1923
Build:
2024
name: ${{ matrix.name }} Python ${{ matrix.python-version }}
@@ -23,7 +27,7 @@ jobs:
2327
fail-fast: false
2428
matrix:
2529
os: [ubuntu-latest, macos-latest, windows-latest]
26-
python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"]
30+
python-version: ${{ github.event_name == 'pull_request' && fromJSON('["3.13"]') || fromJSON('["3.9", "3.10", "3.11", "3.12", "3.13"]') }}
2731
include:
2832
- os: ubuntu-latest
2933
name: Linux

‎src/ipc/candidates/collision_stencil.cpp‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,58 @@
22

33
namespace ipc {
44

5+
VectorMax3d CollisionStencil::compute_normal(
6+
Eigen::ConstRef<VectorMax12d> positions,
7+
bool flip_if_negative,
8+
double* sign) const
9+
{
10+
const int dim = this->dim(positions.size());
11+
12+
VectorMax3d n = compute_unnormalized_normal(positions).normalized();
13+
14+
if (sign != nullptr) {
15+
*sign = (positions.head(dim) - positions.tail(dim)).dot(n) < 0 ? -1 : 1;
16+
}
17+
18+
// Flip the normal if the point is on the negative side.
19+
// Any point on the second object will do, so we use the last point.
20+
if (flip_if_negative
21+
&& (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {
22+
n *= -1;
23+
}
24+
25+
return n;
26+
}
27+
28+
MatrixMax<double, 3, 12> CollisionStencil::compute_normal_jacobian(
29+
Eigen::ConstRef<VectorMax12d> positions, bool flip_if_negative) const
30+
{
31+
const int dim = this->dim(positions.size());
32+
33+
VectorMax3d n = compute_unnormalized_normal(positions);
34+
35+
MatrixMax<double, 3, 12> dn =
36+
compute_unnormalized_normal_jacobian(positions);
37+
38+
#if true
39+
// Derivative of normalization (n̂ = n / ‖n‖)
40+
const double n_norm = n.norm();
41+
n /= n_norm; // n̂
42+
43+
MatrixMax3d A =
44+
(MatrixMax3d::Identity(dim, dim) - n * n.transpose()) / n_norm;
45+
46+
dn = A * dn;
47+
#endif
48+
49+
if (flip_if_negative
50+
&& (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {
51+
dn *= -1;
52+
}
53+
54+
return dn;
55+
}
56+
557
std::ostream& CollisionStencil::write_ccd_query(
658
std::ostream& out,
759
Eigen::ConstRef<VectorMax12d> vertices_t0,

‎src/ipc/candidates/collision_stencil.hpp‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -132,6 +132,40 @@ class CollisionStencil {
132132
return compute_coefficients(dof(vertices, edges, faces));
133133
}
134134

135+
/// @brief Compute the normal of the stencil.
136+
/// @param vertices Collision mesh vertices
137+
/// @param edges Collision mesh edges
138+
/// @param faces Collision mesh faces
139+
/// @param flip_if_negative If true, flip the normal if the point is on the negative side.
140+
/// @param sign If not nullptr, set to the sign of the normal before any flipping.
141+
/// @return Normal of the stencil.
142+
VectorMax3d compute_normal(
143+
Eigen::ConstRef<Eigen::MatrixXd> vertices,
144+
Eigen::ConstRef<Eigen::MatrixXi> edges,
145+
Eigen::ConstRef<Eigen::MatrixXi> faces,
146+
const bool flip_if_negative = true,
147+
double* sign = nullptr) const
148+
{
149+
return compute_normal(
150+
dof(vertices, edges, faces), flip_if_negative, sign);
151+
}
152+
153+
/// @brief Compute the Jacobian of the normal of the stencil.
154+
/// @param vertices Collision mesh vertices
155+
/// @param edges Collision mesh edges
156+
/// @param faces Collision mesh faces
157+
/// @param flip_if_negative If true, flip the normal if the point is on the negative side.
158+
/// @return Jacobian of the normal of the stencil.
159+
MatrixMax<double, 3, 12> compute_normal_jacobian(
160+
Eigen::ConstRef<Eigen::MatrixXd> vertices,
161+
Eigen::ConstRef<Eigen::MatrixXi> edges,
162+
Eigen::ConstRef<Eigen::MatrixXi> faces,
163+
const bool flip_if_negative = true) const
164+
{
165+
return compute_normal_jacobian(
166+
dof(vertices, edges, faces), flip_if_negative);
167+
}
168+
135169
// ----------------------------------------------------------------------
136170
// NOTE: The following functions take stencil vertices as output by dof()
137171
// ----------------------------------------------------------------------
@@ -163,6 +197,24 @@ class CollisionStencil {
163197
virtual VectorMax4d
164198
compute_coefficients(Eigen::ConstRef<VectorMax12d> positions) const = 0;
165199

200+
/// @brief Compute the normal of the stencil.
201+
/// @param positions Stencil's vertex positions.
202+
/// @param flip_if_negative If true, flip the normal if the point is on the negative side.
203+
/// @param sign If not nullptr, set to the sign of the normal before any flipping.
204+
/// @return Normal of the stencil.
205+
VectorMax3d compute_normal(
206+
Eigen::ConstRef<VectorMax12d> positions,
207+
bool flip_if_negative = true,
208+
double* sign = nullptr) const;
209+
210+
/// @brief Compute the Jacobian of the normal of the stencil.
211+
/// @param positions Stencil's vertex positions.
212+
/// @param flip_if_negative If true, flip the normal if the point is on the negative side.
213+
/// @return Jacobian of the normal of the stencil.
214+
MatrixMax<double, 3, 12> compute_normal_jacobian(
215+
Eigen::ConstRef<VectorMax12d> positions,
216+
bool flip_if_negative = true) const;
217+
166218
/// @brief Perform narrow-phase CCD on the candidate.
167219
/// @param[in] vertices_t0 Stencil vertices at the start of the time step.
168220
/// @param[in] vertices_t1 Stencil vertices at the end of the time step.
@@ -189,6 +241,19 @@ class CollisionStencil {
189241
std::ostream& out,
190242
Eigen::ConstRef<VectorMax12d> vertices_t0,
191243
Eigen::ConstRef<VectorMax12d> vertices_t1) const;
244+
245+
protected:
246+
/// @brief Compute the unnormalized normal of the stencil.
247+
/// @param positions Stencil's vertex positions.
248+
/// @return Unnormalized normal of the stencil.
249+
virtual VectorMax3d compute_unnormalized_normal(
250+
Eigen::ConstRef<VectorMax12d> positions) const = 0;
251+
252+
/// @brief Compute the Jacobian of the unnormalized normal of the stencil.
253+
/// @param positions Stencil's vertex positions.
254+
/// @return Jacobian of the unnormalized normal of the stencil.
255+
virtual MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
256+
Eigen::ConstRef<VectorMax12d> positions) const = 0;
192257
};
193258

194259
} // namespace ipc

‎src/ipc/candidates/edge_edge.cpp‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,31 @@ VectorMax4d EdgeEdgeCandidate::compute_coefficients(
9595
return coeffs;
9696
}
9797

98+
VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal(
99+
Eigen::ConstRef<VectorMax12d> positions) const
100+
{
101+
assert(positions.size() == 12);
102+
return (positions.segment<3>(3) - positions.head<3>())
103+
.cross(positions.tail<3>() - positions.segment<3>(6));
104+
}
105+
106+
MatrixMax<double, 3, 12>
107+
EdgeEdgeCandidate::compute_unnormalized_normal_jacobian(
108+
Eigen::ConstRef<VectorMax12d> positions) const
109+
{
110+
assert(positions.size() == 12);
111+
MatrixMax<double, 3, 12> dn(3, 12);
112+
dn.leftCols<3>() =
113+
cross_product_matrix(positions.tail<3>() - positions.segment<3>(6));
114+
dn.middleCols<3>(3) =
115+
cross_product_matrix(positions.segment<3>(6) - positions.tail<3>());
116+
dn.middleCols<3>(6) =
117+
cross_product_matrix(positions.head<3>() - positions.segment<3>(3));
118+
dn.rightCols<3>() =
119+
cross_product_matrix(positions.segment<3>(3) - positions.head<3>());
120+
return dn;
121+
}
122+
98123
bool EdgeEdgeCandidate::ccd(
99124
Eigen::ConstRef<VectorMax12d> vertices_t0,
100125
Eigen::ConstRef<VectorMax12d> vertices_t1,

‎src/ipc/candidates/edge_edge.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,13 @@ class EdgeEdgeCandidate : virtual public CollisionStencil {
8383
index_t edge0_id;
8484
/// @brief ID of the second edge.
8585
index_t edge1_id;
86+
87+
protected:
88+
VectorMax3d compute_unnormalized_normal(
89+
Eigen::ConstRef<VectorMax12d> positions) const override;
90+
91+
MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
92+
Eigen::ConstRef<VectorMax12d> positions) const override;
8693
};
8794

8895
} // namespace ipc

‎src/ipc/candidates/edge_vertex.cpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -77,6 +77,57 @@ VectorMax4d EdgeVertexCandidate::compute_coefficients(
7777
return coeffs;
7878
}
7979

80+
VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal(
81+
Eigen::ConstRef<VectorMax12d> positions) const
82+
{
83+
const int dim = this->dim(positions.size());
84+
85+
if (dim == 2) {
86+
// In 2D, the normal is simply the perpendicular vector to the edge
87+
const Eigen::Vector2d e = positions.tail<2>() - positions.segment<2>(2);
88+
return Eigen::Vector2d(-e.y(), e.x());
89+
}
90+
91+
// Use triple product expansion of the cross product -e × (e × d)
92+
// (https://en.wikipedia.org/wiki/Cross_product#Triple_product_expansion)
93+
// NOTE: This would work in 2D as well, but we handle that case above.
94+
assert(dim == 3);
95+
const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3);
96+
const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3);
97+
return d * e.dot(e) - e * e.dot(d);
98+
}
99+
100+
MatrixMax<double, 3, 12>
101+
EdgeVertexCandidate::compute_unnormalized_normal_jacobian(
102+
Eigen::ConstRef<VectorMax12d> positions) const
103+
{
104+
const int dim = this->dim(positions.size());
105+
if (dim == 2) {
106+
// In 2D, the normal is simply the perpendicular vector to the edge
107+
MatrixMax<double, 3, 12> dn(2, 6);
108+
dn.leftCols<2>().setZero();
109+
dn.middleCols<2>(2) << 0, 1, -1, 0;
110+
dn.rightCols<2>() << 0, -1, 1, 0;
111+
return dn;
112+
}
113+
114+
assert(dim == 3);
115+
const Eigen::Vector3d e = positions.tail<3>() - positions.segment<3>(3);
116+
const Eigen::Vector3d d = positions.head<3>() - positions.segment<3>(3);
117+
118+
const auto I = Eigen::Matrix3d::Identity();
119+
120+
MatrixMax<double, 3, 12> dn(3, 9);
121+
// ∂n/∂x0
122+
dn.leftCols<3>() = e.dot(e) * I - e * e.transpose();
123+
// ∂n/∂x2
124+
dn.rightCols<3>() =
125+
-e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose();
126+
// ∂n/∂x1
127+
dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>();
128+
return dn;
129+
}
130+
80131
bool EdgeVertexCandidate::ccd(
81132
Eigen::ConstRef<VectorMax12d> vertices_t0,
82133
Eigen::ConstRef<VectorMax12d> vertices_t1,

‎src/ipc/candidates/edge_vertex.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,13 @@ class EdgeVertexCandidate : virtual public CollisionStencil {
8080
index_t edge_id;
8181
/// @brief ID of the vertex
8282
index_t vertex_id;
83+
84+
protected:
85+
VectorMax3d compute_unnormalized_normal(
86+
Eigen::ConstRef<VectorMax12d> positions) const override;
87+
88+
MatrixMax<double, 3, 12> compute_unnormalized_normal_jacobian(
89+
Eigen::ConstRef<VectorMax12d> positions) const override;
8390
};
8491

8592
} // namespace ipc

0 commit comments

Comments
 (0)