Skip to content

Commit c400d7e

Browse files
zferguszfergus
committed
Merge branch 'main' into ogc
2 parents 874dad5 + 42cf86e commit c400d7e

File tree

14 files changed

+365
-129
lines changed

14 files changed

+365
-129
lines changed

‎src/ipc/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ add_subdirectory(ccd)
1919
add_subdirectory(collisions)
2020
add_subdirectory(distance)
2121
add_subdirectory(friction)
22+
add_subdirectory(geometry)
2223
add_subdirectory(implicits)
2324
add_subdirectory(ogc)
2425
add_subdirectory(potentials)

‎src/ipc/candidates/collision_stencil.cpp‎

Lines changed: 5 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#include "collision_stencil.hpp"
22

3+
#include <ipc/geometry/normal.hpp>
4+
35
namespace ipc {
46

57
VectorMax3d CollisionStencil::compute_normal(
@@ -30,19 +32,10 @@ MatrixMax<double, 3, 12> CollisionStencil::compute_normal_jacobian(
3032
{
3133
const int dim = this->dim(positions.size());
3234

33-
VectorMax3d n = compute_unnormalized_normal(positions);
34-
35-
MatrixMax<double, 3, 12> dn =
36-
compute_unnormalized_normal_jacobian(positions);
37-
38-
// Derivative of normalization (n̂ = n / ‖n‖)
39-
const double n_norm = n.norm();
40-
n /= n_norm; // n̂
41-
42-
MatrixMax3d A =
43-
(MatrixMax3d::Identity(dim, dim) - n * n.transpose()) / n_norm;
35+
const VectorMax3d n = compute_unnormalized_normal(positions);
4436

45-
dn = A * dn;
37+
MatrixMax<double, 3, 12> dn = normalization_jacobian(n)
38+
* compute_unnormalized_normal_jacobian(positions);
4639

4740
if (flip_if_negative
4841
&& (positions.head(dim) - positions.tail(dim)).dot(n) < 0) {

‎src/ipc/candidates/edge_edge.cpp‎

Lines changed: 7 additions & 12 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/geometry/normal.hpp>
45
#include <ipc/tangent/closest_point.hpp>
56

67
namespace ipc {
@@ -99,25 +100,19 @@ VectorMax3d EdgeEdgeCandidate::compute_unnormalized_normal(
99100
Eigen::ConstRef<VectorMax12d> positions) const
100101
{
101102
assert(positions.size() == 12);
102-
return (positions.segment<3>(3) - positions.head<3>())
103-
.cross(positions.tail<3>() - positions.segment<3>(6));
103+
return edge_edge_unnormalized_normal(
104+
positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6),
105+
positions.tail<3>());
104106
}
105107

106108
MatrixMax<double, 3, 12>
107109
EdgeEdgeCandidate::compute_unnormalized_normal_jacobian(
108110
Eigen::ConstRef<VectorMax12d> positions) const
109111
{
110112
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;
113+
return edge_edge_unnormalized_normal_jacobian(
114+
positions.head<3>(), positions.segment<3>(3), positions.segment<3>(6),
115+
positions.tail<3>());
121116
}
122117

123118
bool EdgeEdgeCandidate::ccd(

‎src/ipc/candidates/edge_vertex.cpp‎

Lines changed: 5 additions & 38 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/geometry/normal.hpp>
45
#include <ipc/tangent/closest_point.hpp>
56

67
#include <iostream>
@@ -81,51 +82,17 @@ VectorMax3d EdgeVertexCandidate::compute_unnormalized_normal(
8182
Eigen::ConstRef<VectorMax12d> positions) const
8283
{
8384
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);
85+
return edge_vertex_unnormalized_normal(
86+
positions.head(dim), positions.segment(dim, dim), positions.tail(dim));
9887
}
9988

10089
MatrixMax<double, 3, 12>
10190
EdgeVertexCandidate::compute_unnormalized_normal_jacobian(
10291
Eigen::ConstRef<VectorMax12d> positions) const
10392
{
10493
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;
94+
return edge_vertex_unnormalized_normal_jacobian(
95+
positions.head(dim), positions.segment(dim, dim), positions.tail(dim));
12996
}
13097

13198
bool EdgeVertexCandidate::ccd(

‎src/ipc/candidates/face_vertex.cpp‎

Lines changed: 5 additions & 8 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/geometry/normal.hpp>
45
#include <ipc/tangent/closest_point.hpp>
56

67
#include <iostream>
@@ -94,8 +95,8 @@ VectorMax3d FaceVertexCandidate::compute_unnormalized_normal(
9495
Eigen::ConstRef<VectorMax12d> positions) const
9596
{
9697
assert(positions.size() == 12);
97-
return (positions.segment<3>(6) - positions.segment<3>(3))
98-
.cross(positions.tail<3>() - positions.segment<3>(3));
98+
return triangle_unnormalized_normal(
99+
positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>());
99100
}
100101

101102
MatrixMax<double, 3, 12>
@@ -105,12 +106,8 @@ FaceVertexCandidate::compute_unnormalized_normal_jacobian(
105106
assert(positions.size() == 12);
106107
MatrixMax<double, 3, 12> dn(3, 12);
107108
dn.leftCols<3>().setZero();
108-
dn.middleCols<3>(3) =
109-
cross_product_matrix(positions.tail<3>() - positions.segment<3>(6));
110-
dn.middleCols<3>(6) =
111-
cross_product_matrix(positions.segment<3>(3) - positions.tail<3>());
112-
dn.rightCols<3>() =
113-
cross_product_matrix(positions.segment<3>(6) - positions.segment<3>(3));
109+
dn.rightCols<9>() = triangle_unnormalized_normal_jacobian(
110+
positions.segment<3>(3), positions.segment<3>(6), positions.tail<3>());
114111
return dn;
115112
}
116113

‎src/ipc/geometry/CMakeLists.txt‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
set(SOURCES
2+
normal.cpp
3+
normal.hpp
4+
)
5+
6+
target_sources(ipc_toolkit PRIVATE ${SOURCES})
7+
8+
################################################################################
9+
# Subfolders
10+
################################################################################

‎src/ipc/geometry/normal.cpp‎

Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
#include "ipc/geometry/normal.hpp"
2+
3+
namespace ipc {
4+
5+
std::tuple<VectorMax3d, MatrixMax3d>
6+
normalization_and_jacobian(Eigen::ConstRef<VectorMax3d> x)
7+
{
8+
const double norm = x.norm();
9+
const VectorMax3d xhat = x / norm;
10+
const auto I = MatrixMax3d::Identity(x.size(), x.size());
11+
const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm;
12+
return std::make_tuple(xhat, J);
13+
}
14+
15+
std::tuple<VectorMax3d, MatrixMax3d, std::array<MatrixMax3d, 3>>
16+
normalization_and_jacobian_and_hessian(Eigen::ConstRef<VectorMax3d> x)
17+
{
18+
// const auto [xhat, J] = normalization_and_jacobian(x);
19+
const double norm = x.norm();
20+
const VectorMax3d xhat = x / norm;
21+
const auto I = MatrixMax3d::Identity(x.size(), x.size());
22+
const MatrixMax3d J = (I - (xhat * xhat.transpose())) / norm;
23+
24+
std::array<MatrixMax3d, 3> H;
25+
for (int i = 0; i < x.size(); i++) {
26+
H[i] = (xhat(i) * J + xhat * J.row(i) + J.col(i) * xhat.transpose())
27+
/ (-norm);
28+
}
29+
30+
return std::make_tuple(xhat, J, H);
31+
}
32+
33+
// --- edge-vertex normal functions -------------------------------------------
34+
35+
VectorMax3d edge_vertex_unnormalized_normal(
36+
Eigen::ConstRef<VectorMax3d> v,
37+
Eigen::ConstRef<VectorMax3d> e0,
38+
Eigen::ConstRef<VectorMax3d> e1)
39+
{
40+
assert(v.size() == e0.size() && v.size() == e1.size());
41+
assert(v.size() == 2 || v.size() == 3);
42+
if (v.size() == 2) {
43+
// In 2D, the normal is simply the perpendicular vector to the edge
44+
const Eigen::Vector2d e = e1.tail<2>() - e0.tail<2>();
45+
return Eigen::Vector2d(-e.y(), e.x());
46+
} else {
47+
// Use triple product expansion of the cross product -e × (e × d)
48+
// (https://en.wikipedia.org/wiki/Cross_product#Triple_product_expansion)
49+
// NOTE: This would work in 2D as well, but we handle that case above.
50+
const Eigen::Vector3d e = e1 - e0;
51+
const Eigen::Vector3d d = v - e0;
52+
return d * e.dot(e) - e * e.dot(d);
53+
}
54+
}
55+
56+
MatrixMax<double, 3, 9> edge_vertex_unnormalized_normal_jacobian(
57+
Eigen::ConstRef<VectorMax3d> v,
58+
Eigen::ConstRef<VectorMax3d> e0,
59+
Eigen::ConstRef<VectorMax3d> e1)
60+
{
61+
assert(v.size() == e0.size() && v.size() == e1.size());
62+
assert(v.size() == 2 || v.size() == 3);
63+
64+
MatrixMax<double, 3, 9> dn(v.size(), 3 * v.size());
65+
66+
if (v.size() == 2) {
67+
// In 2D, the normal is simply the perpendicular vector to the edge
68+
dn.leftCols<2>().setZero();
69+
dn.middleCols<2>(2) << 0, 1, -1, 0;
70+
dn.rightCols<2>() << 0, -1, 1, 0;
71+
return dn;
72+
} else {
73+
const Eigen::Vector3d e = e1 - e0;
74+
const Eigen::Vector3d d = v - e0;
75+
76+
const auto I = Eigen::Matrix3d::Identity();
77+
78+
// ∂n/∂v
79+
dn.leftCols<3>() = e.dot(e) * I - e * e.transpose();
80+
// ∂n/∂e1
81+
dn.rightCols<3>() =
82+
-e.dot(d) * I - e * d.transpose() + (2 * d) * e.transpose();
83+
// ∂n/∂e0
84+
dn.middleCols<3>(3) = -dn.leftCols<3>() - dn.rightCols<3>();
85+
}
86+
87+
return dn;
88+
}
89+
90+
} // namespace ipc

0 commit comments

Comments
 (0)