Skip to content

Commit 38f0798

Browse files
zferguszfergus
committed
Cleaned up tolerance computation
1 parent 1ace9ff commit 38f0798

File tree

1 file changed

+67
-215
lines changed

1 file changed

+67
-215
lines changed

‎tight_inclusion/inclusion_ccd.cpp‎

Lines changed: 67 additions & 215 deletions
Original file line numberDiff line numberDiff line change
@@ -9,76 +9,42 @@
99
namespace inclusion_ccd
1010
{
1111

12-
static Scalar CCD_LENGTH_TOL = 1e-6;
13-
14-
std::array<Vector3d, 2> bbd_4_pts_new(const std::array<Vector3d, 4> &pts)
15-
{
16-
Vector3d min, max;
17-
min = pts[0];
18-
max = pts[0];
19-
for (int i = 1; i < 4; i++)
20-
{
21-
for (int j = 0; j < 3; j++)
22-
{
23-
if (min[j] > pts[i][j])
24-
{
25-
min[j] = pts[i][j];
26-
}
27-
if (max[j] < pts[i][j])
28-
{
29-
max[j] = pts[i][j];
30-
}
31-
}
32-
}
33-
std::array<Vector3d, 2> rst;
34-
rst[0] = min;
35-
rst[1] = max;
36-
return rst;
37-
}
12+
static constexpr Scalar DEFAULT_CCD_LENGTH_TOL = 1e-6;
13+
// #define TIGHT_INCLUSION_MAX_ABS_TOL
14+
#ifdef TIGHT_INCLUSION_MAX_ABS_TOL
15+
static constexpr Scalar CCD_MAX_TIME_TOL = 1e-3;
16+
static constexpr Scalar CCD_MAX_COORD_TOL = 1e-2;
17+
#else
18+
static constexpr Scalar CCD_MAX_TIME_TOL =
19+
std::numeric_limits<double>::infinity();
20+
static constexpr Scalar CCD_MAX_COORD_TOL =
21+
std::numeric_limits<double>::infinity();
22+
#endif
3823

39-
Scalar max_diff(
40-
const Scalar b1min,
41-
const Scalar b1max,
42-
const Scalar b2min,
43-
const Scalar b2max)
24+
inline std::array<Vector3d, 2> bbd_4_pts_new(
25+
const Vector3d &p0,
26+
const Vector3d &p1,
27+
const Vector3d &p2,
28+
const Vector3d &p3)
4429
{
45-
Scalar r = 0;
46-
if (r < b1max - b1min)
47-
r = b1max - b1min;
48-
if (r < b2max - b2min)
49-
r = b2max - b2min;
50-
if (r < fabs(b2min - b1max))
51-
r = fabs(b2min - b1max);
52-
if (r < fabs(b1min - b2max))
53-
r = fabs(b1min - b2max);
54-
return r;
30+
return {
31+
p0.cwiseMin(p1).cwiseMin(p2).cwiseMin(p3),
32+
p0.cwiseMax(p1).cwiseMax(p2).cwiseMax(p3)};
5533
}
5634

5735
// calculate maximum x, y and z diff
5836
Scalar get_max_axis_diff(
5937
const std::array<Vector3d, 2> &b1, const std::array<Vector3d, 2> &b2)
6038
{
61-
62-
Scalar x = max_diff(b1[0][0], b1[1][0], b2[0][0], b2[1][0]);
63-
Scalar y = max_diff(b1[0][1], b1[1][1], b2[0][1], b2[1][1]);
64-
Scalar z = max_diff(b1[0][2], b1[1][2], b2[0][2], b2[1][2]);
65-
return std::max(std::max(x, y), z);
66-
}
67-
68-
Scalar max_linf_dist(const Vector3d &p1, const Vector3d &p2)
69-
{
70-
Scalar r = 0;
71-
for (int i = 0; i < 3; i++)
72-
{
73-
if (r < fabs(p1[i] - p2[i]))
74-
{
75-
r = fabs(p1[i] - p2[i]);
76-
}
77-
}
78-
return r;
39+
return std::max({
40+
(b1[1] - b1[0]).maxCoeff(),
41+
(b2[1] - b2[0]).maxCoeff(),
42+
(b2[0] - b1[1]).cwiseAbs().maxCoeff(),
43+
(b1[0] - b2[1]).cwiseAbs().maxCoeff(),
44+
});
7945
}
8046

81-
Scalar max_linf_4(
47+
inline Scalar max_linf_4(
8248
const Vector3d &p1,
8349
const Vector3d &p2,
8450
const Vector3d &p3,
@@ -88,20 +54,11 @@ namespace inclusion_ccd
8854
const Vector3d &p3e,
8955
const Vector3d &p4e)
9056
{
91-
Scalar r = 0, temp = 0;
92-
temp = max_linf_dist(p1e, p1);
93-
if (r < temp)
94-
r = temp;
95-
temp = max_linf_dist(p2e, p2);
96-
if (r < temp)
97-
r = temp;
98-
temp = max_linf_dist(p3e, p3);
99-
if (r < temp)
100-
r = temp;
101-
temp = max_linf_dist(p4e, p4);
102-
if (r < temp)
103-
r = temp;
104-
return r;
57+
return std::max(
58+
{(p1e - p1).lpNorm<Eigen::Infinity>(),
59+
(p2e - p2).lpNorm<Eigen::Infinity>(),
60+
(p3e - p3).lpNorm<Eigen::Infinity>(),
61+
(p4e - p4).lpNorm<Eigen::Infinity>()});
10562
}
10663

10764
Vector3d compute_face_vertex_tolerance_3d_new(
@@ -115,67 +72,26 @@ namespace inclusion_ccd
11572
const Vector3d &f2e,
11673
const Scalar tolerance)
11774
{
118-
Vector3d p000 = vs - f0s, p001 = vs - f2s,
119-
p011 = vs - (f1s + f2s - f0s), p010 = vs - f1s;
120-
Vector3d p100 = ve - f0e, p101 = ve - f2e,
121-
p111 = ve - (f1e + f2e - f0e), p110 = ve - f1e;
122-
Scalar dl = 0;
123-
Scalar edge0_length = 0;
124-
Scalar edge1_length = 0;
125-
dl = 3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110);
126-
edge0_length =
75+
const Vector3d p000 = vs - f0s;
76+
const Vector3d p001 = vs - f2s;
77+
const Vector3d p011 = vs - (f1s + f2s - f0s);
78+
const Vector3d p010 = vs - f1s;
79+
const Vector3d p100 = ve - f0e;
80+
const Vector3d p101 = ve - f2e;
81+
const Vector3d p111 = ve - (f1e + f2e - f0e);
82+
const Vector3d p110 = ve - f1e;
83+
84+
Scalar dl =
85+
3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110);
86+
Scalar edge0_length =
12787
3 * max_linf_4(p000, p100, p101, p001, p010, p110, p111, p011);
128-
edge1_length =
88+
Scalar edge1_length =
12989
3 * max_linf_4(p000, p100, p110, p010, p001, p101, p111, p011);
130-
// Scalar diag=max_linf_4(
131-
// p000,p100,p110,p010,
132-
// p111,p011,p001,p101);
133-
134-
// std::array<Vector3d,2>
135-
// t0box=bbd_4_pts_new({{p000,p001,p010,p011}});
136-
// std::array<Vector3d,2>
137-
// t1box=bbd_4_pts_new({{p100,p101,p110,p111}});
138-
// std::array<Vector3d,2>
139-
// u0box=bbd_4_pts_new({{p000,p001,p100,p101}});
140-
// std::array<Vector3d,2>
141-
// u1box=bbd_4_pts_new({{p010,p011,p110,p111}});
142-
// std::array<Vector3d,2>
143-
// v0box=bbd_4_pts_new({{p000,p100,p010,p110}});
144-
// std::array<Vector3d,2>
145-
// v1box=bbd_4_pts_new({{p001,p101,p011,p111}});
146-
// dl=get_max_axis_diff(t0box,t1box);
147-
// edge0_length=get_max_axis_diff(u0box,u1box);
148-
// edge1_length=get_max_axis_diff(v0box,v1box);
149-
// for(int i=0;i<3;i++){
150-
// if(dl<fabs(ve[i]-vs[i]))
151-
// dl=fabs(ve[i]-vs[i]);
152-
153-
// if(dl<fabs(f0e[i]-f0s[i]))
154-
// dl=fabs(f0e[i]-f0s[i]);
155-
156-
// if(dl<fabs(f1e[i]-f1s[i]))
157-
// dl=fabs(f1e[i]-f1s[i]);
158-
159-
// if(dl<fabs(f2e[i]-f2s[i]))
160-
// dl=fabs(f2e[i]-f2s[i]);
161-
162-
// if(edge0_length<fabs(f1s[i] - f0s[i]))
163-
// edge0_length=fabs(f1s[i] - f0s[i]);
164-
165-
// if(edge0_length<fabs(f1e[i] - f0e[i]))
166-
// edge0_length=fabs(f1e[i] - f0e[i]);
167-
168-
// if(edge1_length<fabs(f2s[i]-f0s[i]))
169-
// edge1_length=fabs(f2s[i]-f0s[i]);
170-
171-
// if(edge1_length<fabs(f2e[i]-f0e[i]))
172-
// edge1_length=fabs(f2e[i]-f0e[i]);
173-
// }
174-
// //Scalar edge_length = std::max(edge0_length, edge1_length);
175-
// c00=0;c10=0;c20=0;
176-
// c00=dl;c10=edge0_length;c20=edge1_length;
90+
17791
return Vector3d(
178-
tolerance / dl, tolerance / edge0_length, tolerance / edge1_length);
92+
std::min(tolerance / dl, CCD_MAX_TIME_TOL), // time tolerance
93+
std::min(tolerance / edge0_length, CCD_MAX_COORD_TOL),
94+
std::min(tolerance / edge1_length, CCD_MAX_COORD_TOL));
17995
}
18096

18197
Vector3d compute_edge_edge_tolerance_new(
@@ -190,89 +106,26 @@ namespace inclusion_ccd
190106
const Scalar tolerance)
191107
{
192108

193-
Vector3d p000 = edge0_vertex0_start - edge1_vertex0_start,
194-
p001 = edge0_vertex0_start - edge1_vertex1_start,
195-
p011 = edge0_vertex1_start - edge1_vertex1_start,
196-
p010 = edge0_vertex1_start - edge1_vertex0_start;
197-
Vector3d p100 = edge0_vertex0_end - edge1_vertex0_end,
198-
p101 = edge0_vertex0_end - edge1_vertex1_end,
199-
p111 = edge0_vertex1_end - edge1_vertex1_end,
200-
p110 = edge0_vertex1_end - edge1_vertex0_end;
201-
Scalar dl = 0;
202-
Scalar edge0_length = 0;
203-
Scalar edge1_length = 0;
204-
// {
205-
// std::array<Vector3d,2>
206-
// t0box=bbd_4_pts_new({{p000,p001,p010,p011}});
207-
// std::array<Vector3d,2>
208-
// t1box=bbd_4_pts_new({{p100,p101,p110,p111}});
209-
// std::array<Vector3d,2>
210-
// u0box=bbd_4_pts_new({{p000,p001,p100,p101}});
211-
// std::array<Vector3d,2>
212-
// u1box=bbd_4_pts_new({{p010,p011,p110,p111}});
213-
// std::array<Vector3d,2>
214-
// v0box=bbd_4_pts_new({{p000,p100,p010,p110}});
215-
// std::array<Vector3d,2>
216-
// v1box=bbd_4_pts_new({{p001,p101,p011,p111}});
217-
// dl=get_max_axis_diff(t0box,t1box);
218-
// edge0_length=get_max_axis_diff(u0box,u1box);
219-
// edge1_length=get_max_axis_diff(v0box,v1box);
220-
// }
221-
222-
dl = 3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110);
223-
edge0_length =
109+
const Vector3d p000 = edge0_vertex0_start - edge1_vertex0_start;
110+
const Vector3d p001 = edge0_vertex0_start - edge1_vertex1_start;
111+
const Vector3d p011 = edge0_vertex1_start - edge1_vertex1_start;
112+
const Vector3d p010 = edge0_vertex1_start - edge1_vertex0_start;
113+
const Vector3d p100 = edge0_vertex0_end - edge1_vertex0_end;
114+
const Vector3d p101 = edge0_vertex0_end - edge1_vertex1_end;
115+
const Vector3d p111 = edge0_vertex1_end - edge1_vertex1_end;
116+
const Vector3d p110 = edge0_vertex1_end - edge1_vertex0_end;
117+
118+
Scalar dl =
119+
3 * max_linf_4(p000, p001, p011, p010, p100, p101, p111, p110);
120+
Scalar edge0_length =
224121
3 * max_linf_4(p000, p100, p101, p001, p010, p110, p111, p011);
225-
edge1_length =
122+
Scalar edge1_length =
226123
3 * max_linf_4(p000, p100, p110, p010, p001, p101, p111, p011);
227-
// for(int i=0;i<3;i++){
228-
// if(fabs(p000[i]-p100[i])>dl)
229-
// dl=fabs(p000[i]-p100[i]);
230-
// if(fabs(p001[i]-p101[i])>dl)
231-
// dl=fabs(p001[i]-p101[i]);
232-
// if(fabs(p010[i]-p110[i])>dl)
233-
// dl=fabs(p010[i]-p110[i]);
234-
// if(fabs(p011[i]-p111[i])>dl)
235-
// dl=fabs(p011[i]-p111[i]);
236-
// }
237-
238-
// Scalar dl=0;
239-
// Scalar edge0_length=0;
240-
// Scalar edge1_length=0;
241-
// for(int i=0;i<3;i++){
242-
// if(dl<fabs(edge0_vertex0_end[i]-edge0_vertex0_start[i]))
243-
// dl=fabs(edge0_vertex0_end[i]-edge0_vertex0_start[i]);
244-
245-
// if(dl<fabs(edge0_vertex1_end[i]-edge0_vertex1_start[i]))
246-
// dl=fabs(edge0_vertex1_end[i]-edge0_vertex1_start[i]);
247-
248-
// if(dl<fabs(edge1_vertex0_end[i]-edge1_vertex0_start[i]))
249-
// dl=fabs(edge1_vertex0_end[i]-edge1_vertex0_start[i]);
250-
251-
// if(dl<fabs(edge1_vertex1_end[i]-edge1_vertex1_start[i]))
252-
// dl=fabs(edge1_vertex1_end[i]-edge1_vertex1_start[i]);
253-
254-
// if(edge0_length<fabs(edge0_vertex1_start[i] -
255-
// edge0_vertex0_start[i]))
256-
// edge0_length=fabs(edge0_vertex1_start[i] -
257-
// edge0_vertex0_start[i]);
258-
259-
// if(edge0_length<fabs(edge0_vertex1_end[i] -
260-
// edge0_vertex0_end[i]))
261-
// edge0_length=fabs(edge0_vertex1_end[i] -
262-
// edge0_vertex0_end[i]);
263-
264-
// if(edge1_length<fabs(edge1_vertex1_start[i] -
265-
// edge1_vertex0_start[i]))
266-
// edge1_length=fabs(edge1_vertex1_start[i] -
267-
// edge1_vertex0_start[i]);
268-
269-
// if(edge1_length<fabs(edge1_vertex1_end[i] -
270-
// edge1_vertex0_end[i]))
271-
// edge1_length=fabs(edge1_vertex1_end[i] -
272-
// edge1_vertex0_end[i]);
273-
// }
124+
274125
return Vector3d(
275-
tolerance / dl, tolerance / edge0_length, tolerance / edge1_length);
126+
std::min(tolerance / dl, CCD_MAX_TIME_TOL),
127+
std::min(tolerance / edge0_length, CCD_MAX_COORD_TOL),
128+
std::min(tolerance / edge1_length, CCD_MAX_COORD_TOL));
276129
}
277130

278131
bool edgeEdgeCCD_double(
@@ -547,7 +400,7 @@ namespace inclusion_ccd
547400
{
548401

549402
Vector3d tol = compute_edge_edge_tolerance_new(
550-
a0s, a1s, b0s, b1s, a0e, a1e, b0e, b1e, CCD_LENGTH_TOL);
403+
a0s, a1s, b0s, b1s, a0e, a1e, b0e, b1e, DEFAULT_CCD_LENGTH_TOL);
551404

552405
//////////////////////////////////////////////////////////
553406
// TODO this should be the error of the whole mesh
@@ -596,11 +449,10 @@ namespace inclusion_ccd
596449
const Scalar ms,
597450
Scalar &toi)
598451
{
599-
600452
Vector3d tol = compute_face_vertex_tolerance_3d_new(
601453
vertex_start, face_vertex0_start, face_vertex1_start,
602454
face_vertex2_start, vertex_end, face_vertex0_end, face_vertex1_end,
603-
face_vertex2_end, CCD_LENGTH_TOL);
455+
face_vertex2_end, DEFAULT_CCD_LENGTH_TOL);
604456
// std::cout<<"get tolerance successfully"<<std::endl;
605457
//////////////////////////////////////////////////////////
606458
// TODO this should be the error of the whole mesh

0 commit comments

Comments
 (0)