-
Notifications
You must be signed in to change notification settings - Fork 128
/
makelevelset3.cpp
184 lines (176 loc) · 8.14 KB
/
makelevelset3.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
#include "makelevelset3.h"
// find distance x0 is from segment x1-x2
static float point_segment_distance(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2)
{
Vec3f dx(x2-x1);
double m2=mag2(dx);
// find parameter value of closest point on segment
float s12=(float)(dot(x2-x0, dx)/m2);
if(s12<0){
s12=0;
}else if(s12>1){
s12=1;
}
// and find the distance
return dist(x0, s12*x1+(1-s12)*x2);
}
// find distance x0 is from triangle x1-x2-x3
static float point_triangle_distance(const Vec3f &x0, const Vec3f &x1, const Vec3f &x2, const Vec3f &x3)
{
// first find barycentric coordinates of closest point on infinite plane
Vec3f x13(x1-x3), x23(x2-x3), x03(x0-x3);
float m13=mag2(x13), m23=mag2(x23), d=dot(x13,x23);
float invdet=1.f/max(m13*m23-d*d,1e-30f);
float a=dot(x13,x03), b=dot(x23,x03);
// the barycentric coordinates themselves
float w23=invdet*(m23*a-d*b);
float w31=invdet*(m13*b-d*a);
float w12=1-w23-w31;
if(w23>=0 && w31>=0 && w12>=0){ // if we're inside the triangle
return dist(x0, w23*x1+w31*x2+w12*x3);
}else{ // we have to clamp to one of the edges
if(w23>0) // this rules out edge 2-3 for us
return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x1,x3));
else if(w31>0) // this rules out edge 1-3
return min(point_segment_distance(x0,x1,x2), point_segment_distance(x0,x2,x3));
else // w12 must be >0, ruling out edge 1-2
return min(point_segment_distance(x0,x1,x3), point_segment_distance(x0,x2,x3));
}
}
static void check_neighbour(const std::vector<Vec3ui> &tri, const std::vector<Vec3f> &x,
Array3f &phi, Array3i &closest_tri,
const Vec3f &gx, int i0, int j0, int k0, int i1, int j1, int k1)
{
if(closest_tri(i1,j1,k1)>=0){
unsigned int p, q, r; assign(tri[closest_tri(i1,j1,k1)], p, q, r);
float d=point_triangle_distance(gx, x[p], x[q], x[r]);
if(d<phi(i0,j0,k0)){
phi(i0,j0,k0)=d;
closest_tri(i0,j0,k0)=closest_tri(i1,j1,k1);
}
}
}
static void sweep(const std::vector<Vec3ui> &tri, const std::vector<Vec3f> &x,
Array3f &phi, Array3i &closest_tri, const Vec3f &origin, float dx,
int di, int dj, int dk)
{
int i0, i1;
if(di>0){ i0=1; i1=phi.ni; }
else{ i0=phi.ni-2; i1=-1; }
int j0, j1;
if(dj>0){ j0=1; j1=phi.nj; }
else{ j0=phi.nj-2; j1=-1; }
int k0, k1;
if(dk>0){ k0=1; k1=phi.nk; }
else{ k0=phi.nk-2; k1=-1; }
for(int k=k0; k!=k1; k+=dk) for(int j=j0; j!=j1; j+=dj) for(int i=i0; i!=i1; i+=di){
Vec3f gx(i*dx+origin[0], j*dx+origin[1], k*dx+origin[2]);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j, k);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j-dj, k);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j-dj, k);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j, k-dk);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j, k-dk);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i, j-dj, k-dk);
check_neighbour(tri, x, phi, closest_tri, gx, i, j, k, i-di, j-dj, k-dk);
}
}
// calculate twice signed area of triangle (0,0)-(x1,y1)-(x2,y2)
// return an SOS-determined sign (-1, +1, or 0 only if it's a truly degenerate triangle)
static int orientation(double x1, double y1, double x2, double y2, double &twice_signed_area)
{
twice_signed_area=y1*x2-x1*y2;
if(twice_signed_area>0) return 1;
else if(twice_signed_area<0) return -1;
else if(y2>y1) return 1;
else if(y2<y1) return -1;
else if(x1>x2) return 1;
else if(x1<x2) return -1;
else return 0; // only true when x1==x2 and y1==y2
}
// robust test of (x0,y0) in the triangle (x1,y1)-(x2,y2)-(x3,y3)
// if true is returned, the barycentric coordinates are set in a,b,c.
static bool point_in_triangle_2d(double x0, double y0,
double x1, double y1, double x2, double y2, double x3, double y3,
double& a, double& b, double& c)
{
x1-=x0; x2-=x0; x3-=x0;
y1-=y0; y2-=y0; y3-=y0;
int signa=orientation(x2, y2, x3, y3, a);
if(signa==0) return false;
int signb=orientation(x3, y3, x1, y1, b);
if(signb!=signa) return false;
int signc=orientation(x1, y1, x2, y2, c);
if(signc!=signa) return false;
double sum=a+b+c;
assert(sum!=0); // if the SOS signs match and are nonkero, there's no way all of a, b, and c are zero.
a/=sum;
b/=sum;
c/=sum;
return true;
}
void make_level_set3(const std::vector<Vec3ui> &tri, const std::vector<Vec3f> &x,
const Vec3f &origin, float dx, int ni, int nj, int nk,
Array3f &phi, const int exact_band)
{
phi.resize(ni, nj, nk);
phi.assign((ni+nj+nk)*dx); // upper bound on distance
Array3i closest_tri(ni, nj, nk, -1);
Array3i intersection_count(ni, nj, nk, 0); // intersection_count(i,j,k) is # of tri intersections in (i-1,i]x{j}x{k}
// we begin by initializing distances near the mesh, and figuring out intersection counts
Vec3f ijkmin, ijkmax;
for(unsigned int t=0; t<tri.size(); ++t){
unsigned int p, q, r; assign(tri[t], p, q, r);
// coordinates in grid to high precision
double fip=((double)x[p][0]-origin[0])/dx, fjp=((double)x[p][1]-origin[1])/dx, fkp=((double)x[p][2]-origin[2])/dx;
double fiq=((double)x[q][0]-origin[0])/dx, fjq=((double)x[q][1]-origin[1])/dx, fkq=((double)x[q][2]-origin[2])/dx;
double fir=((double)x[r][0]-origin[0])/dx, fjr=((double)x[r][1]-origin[1])/dx, fkr=((double)x[r][2]-origin[2])/dx;
// do distances nearby
int i0=clamp(int(min(fip,fiq,fir))-exact_band, 0, ni-1), i1=clamp(int(max(fip,fiq,fir))+exact_band+1, 0, ni-1);
int j0=clamp(int(min(fjp,fjq,fjr))-exact_band, 0, nj-1), j1=clamp(int(max(fjp,fjq,fjr))+exact_band+1, 0, nj-1);
int k0=clamp(int(min(fkp,fkq,fkr))-exact_band, 0, nk-1), k1=clamp(int(max(fkp,fkq,fkr))+exact_band+1, 0, nk-1);
for(int k=k0; k<=k1; ++k) for(int j=j0; j<=j1; ++j) for(int i=i0; i<=i1; ++i){
Vec3f gx(i*dx+origin[0], j*dx+origin[1], k*dx+origin[2]);
float d=point_triangle_distance(gx, x[p], x[q], x[r]);
if(d<phi(i,j,k)){
phi(i,j,k)=d;
closest_tri(i,j,k)=t;
}
}
// and do intersection counts
j0=clamp((int)std::ceil(min(fjp,fjq,fjr)), 0, nj-1);
j1=clamp((int)std::floor(max(fjp,fjq,fjr)), 0, nj-1);
k0=clamp((int)std::ceil(min(fkp,fkq,fkr)), 0, nk-1);
k1=clamp((int)std::floor(max(fkp,fkq,fkr)), 0, nk-1);
for(int k=k0; k<=k1; ++k) for(int j=j0; j<=j1; ++j){
double a, b, c;
if(point_in_triangle_2d(j, k, fjp, fkp, fjq, fkq, fjr, fkr, a, b, c)){
double fi=a*fip+b*fiq+c*fir; // intersection i coordinate
int i_interval=int(std::ceil(fi)); // intersection is in (i_interval-1,i_interval]
if(i_interval<0) ++intersection_count(0, j, k); // we enlarge the first interval to include everything to the -x direction
else if(i_interval<ni) ++intersection_count(i_interval,j,k);
// we ignore intersections that are beyond the +x side of the grid
}
}
}
// and now we fill in the rest of the distances with fast sweeping
for(unsigned int pass=0; pass<2; ++pass){
sweep(tri, x, phi, closest_tri, origin, dx, +1, +1, +1);
sweep(tri, x, phi, closest_tri, origin, dx, -1, -1, -1);
sweep(tri, x, phi, closest_tri, origin, dx, +1, +1, -1);
sweep(tri, x, phi, closest_tri, origin, dx, -1, -1, +1);
sweep(tri, x, phi, closest_tri, origin, dx, +1, -1, +1);
sweep(tri, x, phi, closest_tri, origin, dx, -1, +1, -1);
sweep(tri, x, phi, closest_tri, origin, dx, +1, -1, -1);
sweep(tri, x, phi, closest_tri, origin, dx, -1, +1, +1);
}
// then figure out signs (inside/outside) from intersection counts
for(int k=0; k<nk; ++k) for(int j=0; j<nj; ++j){
int total_count=0;
for(int i=0; i<ni; ++i){
total_count+=intersection_count(i,j,k);
if(total_count%2==1){ // if parity of intersections so far is odd,
phi(i,j,k)=-phi(i,j,k); // we are inside the mesh
}
}
}
}